#include "user_global.h"
#include "usbd_cdc_vcp.h"
#include "delay.h"

extern "C" {
    uint8_t VCP_SendData(USB_OTG_CORE_HANDLE * pdev, uint8_t * pbuf, uint32_t buf_len);
} /* extern C */

extern USB_OTG_CORE_HANDLE USB_OTG_dev;
uint32_t result;
//TODO:11 云台、底盘、发射、系统、裁判、决策、调试、超级电容功率控制、待优化、手柄、修改
SystemMonitor G_system_monitor;
RefereeMonitor G_referee_monitor;
Led G_led;
Gimbal G_gimbal;
DJIRC G_djirc;
Vofa G_vofa;
AimAssist G_aim_assist;
Navigation G_navigation;
Referee G_referee;
SentryRobot G_sentry;
ControlMode G_control_mode;

//TODO:【监测】遥控状态监测,在接收帧率较低时保持默认值
/**
 *@brief Remote control state monitor
 *
 *@param
 */
void RemoteControlMonitor(void)
{
    if (G_system_monitor.UART1_rx_fps < 50)
    {
        G_djirc.channel.Ch0 = RC_CH_VALUE_OFFSET;
        G_djirc.channel.Ch1 = RC_CH_VALUE_OFFSET;
        G_djirc.channel.Ch2 = RC_CH_VALUE_OFFSET;
        G_djirc.channel.Ch3 = RC_CH_VALUE_OFFSET;
        G_djirc.channel.Ch4 = RC_CH_VALUE_OFFSET;
    }
}


//TODO:【监测】电机状态监测，利用LED体现电机状态
/**
 *@brief Motor state monitor
 *
 *@param
 */
void MotorMonitor(void)
{
    static uint32_t times = 0;
    times++;
    if (G_system_monitor.CAN1_rx_fps < 5000 && G_system_monitor.CAN2_rx_fps < 5000)
    {
        if (times % 3 == 0)
        {
            G_led.ToggleGreen();
        }
    }
    else if (G_system_monitor.CAN2_rx_fps < 5000)
    {
        if (times % 2 == 0)
        {
            G_led.ToggleGreen();
        }
    }
    else if (G_system_monitor.CAN1_rx_fps < 5000)
    {
        G_led.ToggleGreen();
    }
    else
    {
        G_led.SetGreen(false);
    }
    // Reset times to prevent overflow 避免无限制累加
    if (times >= 8)
    {
        times = 0;
    }
}


//TODO:【监测】通信状态监测，利用LED体现通信状态
/**
 *@brief Communication state monitor
 *
 *@param
 */
void CommunicationMonitor(void)
{
    static uint32_t times = 0;
    times++;
    if (G_system_monitor.UART3_rx_fps < 50)
    {
        if (times % 10 < 3)
        {
            G_led.SetRed(true);
            G_led.SetBlue(false);
            G_led.SetGreen(false);
        }
    }
    if (G_system_monitor.UART6_rx_fps < 50)
    {
        if (times % 10 >= 3 && times % 10 <= 6)
        {
            G_led.SetBlue(true);
            G_led.SetRed(false);
            G_led.SetGreen(false);
        }
    }
    if (G_system_monitor.UART6_rx_fps < 500)
    {
        if (times % 10 > 6)
        {
            G_led.SetGreen(true);
            G_led.SetRed(false);
            G_led.SetBlue(false);
        }
    }
}



/**
 *@brief visualize navigation datas
 *
 *@param
 */
void VisualizeNavigationData(void)
{
    G_vofa.m_data_send_frame.m_data[0] = G_navigation.m_data_receive_frame.m_data_f[0];
    G_vofa.m_data_send_frame.m_data[1] = G_navigation.m_data_receive_frame.m_data_f[1];
    G_vofa.m_data_send_frame.m_data[2] = G_navigation.m_data_receive_frame.m_data_f[2];
    G_vofa.m_data_send_frame.m_data[3] = G_navigation.m_data_receive_frame.m_data_f[3];
    G_vofa.m_data_send_frame.m_data[4] = G_navigation.m_data_receive_frame.m_data_f[4];
    G_vofa.m_data_send_frame.m_data[5] = G_navigation.m_data_receive_frame.m_data_f[5];
    G_vofa.m_data_send_frame.m_data[6] = G_navigation.m_data_receive_frame.m_data_f[6];
    G_vofa.m_data_send_frame.m_data[7] = G_navigation.m_data_receive_frame.m_data_f[7];
    G_vofa.m_data_send_frame.m_data[8] = G_navigation.m_data_receive_frame.m_data_f[8];
    G_vofa.m_data_send_frame.m_data[9] = G_navigation.m_data_receive_frame.m_data_f[9];
    G_vofa.m_data_send_frame.m_data[10] = G_navigation.m_data_receive_frame.m_data_f[10];
    G_vofa.m_data_send_frame.m_data[11] = G_navigation.m_data_receive_frame.m_data_c[0];
    G_vofa.m_data_send_frame.m_data[12] = G_navigation.m_data_receive_frame.m_data_c[1];
    G_vofa.m_data_send_frame.m_data[13] = G_navigation.m_data_receive_frame.m_data_c[2];
    // G_vofa.m_data_send_frame.m_data[14] = G_navigation.m_data_receive_frame.m_data_f[14];
    G_vofa.m_data_send_frame.m_data[15] = G_sentry.speed_x_des;
    G_vofa.m_data_send_frame.m_data[16] = G_sentry.speed_y_des;
    G_vofa.m_data_send_frame.m_data[17] = G_sentry.speed_w_des;
    G_vofa.m_data_send_frame.m_data[14] = G_sentry.m_chassis_speed2chassis_angle_target;
}


//TODO：【底盘】可视化底盘数据，发到VOFA上进行观测
/**
 *@brief visualize chassis datas
 *
 *@param
 */
void VisualizeChassisData(void)
{
    // G_vofa.m_data_send_frame.m_data[0] = G_sentry.m_chassis_speed*cos(G_sentry.m_chassis_speed2world_angle/RADIAN2DEGREE_VALUE);
    // G_vofa.m_data_send_frame.m_data[1] = G_sentry.m_chassis_speed*sin(G_sentry.m_chassis_speed2world_angle/RADIAN2DEGREE_VALUE);

    G_vofa.m_data_send_frame.m_data[0] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FL_MOTOR]->m_speed_target;
    G_vofa.m_data_send_frame.m_data[1] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FL_MOTOR]->m_speed_current;
    G_vofa.m_data_send_frame.m_data[2] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FR_MOTOR]->m_speed_target;
    G_vofa.m_data_send_frame.m_data[3] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FR_MOTOR]->m_speed_current;
    G_vofa.m_data_send_frame.m_data[4] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BL_MOTOR]->m_speed_target;
    G_vofa.m_data_send_frame.m_data[5] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BL_MOTOR]->m_speed_current;
    G_vofa.m_data_send_frame.m_data[6] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BR_MOTOR]->m_speed_target;
    G_vofa.m_data_send_frame.m_data[7] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BR_MOTOR]->m_speed_current;
    // G_vofa.m_data_send_frame.m_data[8] = G_sentry.chassis_steer_motor[SentryRobot::CHASSIS_S_BL_MOTOR]->m_angle_target;
    // G_vofa.m_data_send_frame.m_data[9] = G_sentry.chassis_steer_motor[SentryRobot::CHASSIS_S_BL_MOTOR]->m_angle_current;
    // G_vofa.m_data_send_frame.m_data[10] = G_sentry.chassis_steer_motor[SentryRobot::CHASSIS_S_BR_MOTOR]->m_angle_target;
    // G_vofa.m_data_send_frame.m_data[11] = G_sentry.chassis_steer_motor[SentryRobot::CHASSIS_S_BR_MOTOR]->m_angle_current;

    G_vofa.m_data_send_frame.m_data[8] = G_sentry.speed_x_des;
    G_vofa.m_data_send_frame.m_data[9] = G_sentry.speed_y_des;
    G_vofa.m_data_send_frame.m_data[10] = G_sentry.speed_w_des;

    G_vofa.m_data_send_frame.m_data[11] = G_sentry.yaw_direction_speed;
    G_vofa.m_data_send_frame.m_data[12] = G_sentry.m_chassis2world_angle_target;
    G_vofa.m_data_send_frame.m_data[13] = G_sentry.m_chassis2world_angle_current;
    G_vofa.m_data_send_frame.m_data[14] = G_sentry.yaw_vertical_speed;
    G_vofa.m_data_send_frame.m_data[15] = G_sentry.capacitor->m_power_now ;
    G_vofa.m_data_send_frame.m_data[16] = G_sentry.capacitor->m_power_buffer_now;
    G_vofa.m_data_send_frame.m_data[17] = G_sentry.capacitor->m_power_limit;

    //float m_power_now;//当前裁判系统测得底盘使用的功率
    //float m_power_buffer_next;//相对于当前下一个时刻缓冲能
    //float m_power_buffer_now;//当前缓冲能
//    G_vofa.m_data_send_frame.m_data[14] = G_sentry.m_robot_radar_world_yaw;
//			G_vofa.m_data_send_frame.m_data[14] = G_referee.PowerHeatData.chassis_power;

 //   G_vofa.m_data_send_frame.m_data[15] = G_sentry.m_chassis_speed;
//    G_vofa.m_data_send_frame.m_data[16] = G_sentry.m_chassis_speed2world_angle;
 //  G_vofa.m_data_send_frame.m_data[16] = G_sentry.capacitor->m_pow_in;

    // G_vofa.m_data_send_frame.m_data[17] = G_sentry.m_chassis_spin_speed;

    // G_vofa.m_data_send_frame.m_data[17] = sqrt(pow(G_navigation.m_data_receive_frame.m_data_f[0],2) + pow(G_navigation.m_data_receive_frame.m_data_f[2],2));

    
    
    // G_vofa.m_data_send_frame.m_data[19] = G_sentry.m_chassislimit_speedtrack_k;
    // G_vofa.m_data_send_frame.m_data[18] = G_sentry.test_box2;
    //G_vofa.m_data_send_frame.m_data[17] = G_sentry.capacitor->m_volt_out;
    // G_vofa.m_data_send_frame.m_data[17] = G_sentry.capacitor->CAP_Cur_Coe;
    G_vofa.m_data_send_frame.m_data[18] = G_sentry.capacitor->m_pow_out;
    G_vofa.m_data_send_frame.m_data[19] = G_sentry.capacitor->m_cap_vol;


    // G_vofa.m_data_send_frame.m_data[17] = G_system_monitor.Gimbal_Receive_fps;
    // G_vofa.m_data_send_frame.m_data[18] = G_system_monitor.CAN1_rx_fps;
    // G_vofa.m_data_send_frame.m_data[19] = G_system_monitor.CAN2_rx_fps;
}


//TODO:【云台】数据可视化，发到VOFA上进行观测
/**
 *@brief visualize  datas
 *
 *@param
 */
void VisualizeGimbalData(void)
{
    // G_vofa.m_data_send_frame.m_data[0] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current;
    // G_vofa.m_data_send_frame.m_data[1] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current;
    // G_vofa.m_data_send_frame.m_data[2] = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current;

    // // Gimbal angle target
    // G_vofa.m_data_send_frame.m_data[3] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_target;
    // G_vofa.m_data_send_frame.m_data[4] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_target;
    // G_vofa.m_data_send_frame.m_data[5] = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_target;

    G_vofa.m_data_send_frame.m_data[0] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_target;
    G_vofa.m_data_send_frame.m_data[1] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current;
    G_vofa.m_data_send_frame.m_data[2] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_td->m_x1;

    G_vofa.m_data_send_frame.m_data[3] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_target;
    G_vofa.m_data_send_frame.m_data[4] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current;
    G_vofa.m_data_send_frame.m_data[5] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_td->m_x1;

    G_vofa.m_data_send_frame.m_data[6] = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_target;
    G_vofa.m_data_send_frame.m_data[7] = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current;
    G_vofa.m_data_send_frame.m_data[8] = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_td->m_x1;

    G_vofa.m_data_send_frame.m_data[9] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_speed_pid->m_output;
    G_vofa.m_data_send_frame.m_data[10] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_speed_pid->m_output;
    G_vofa.m_data_send_frame.m_data[11] = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_speed_pid->m_output;

    G_vofa.m_data_send_frame.m_data[12] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_speed_target;
    G_vofa.m_data_send_frame.m_data[13] = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_speed_target;
    G_vofa.m_data_send_frame.m_data[14] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_speed_target;

    G_vofa.m_data_send_frame.m_data[15] = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_target;
    G_vofa.m_data_send_frame.m_data[16] = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_current;
    

    float pitch_speed = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_speed_current;
    float pitch = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current;
    if(fabs(pitch_speed>5))
        G_vofa.m_data_send_frame.m_data[17] = LPF(G_vofa.m_data_send_frame.m_data[16], pitch, 1000, 200);
    else
        G_vofa.m_data_send_frame.m_data[17] = LPF(G_vofa.m_data_send_frame.m_data[16], pitch, 1000, 20);
    //G_vofa.m_data_send_frame.m_data[18] = pitch_speed;
    //G_vofa.m_data_send_frame.m_data[19] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_speed_target;
    G_vofa.m_data_send_frame.m_data[18] = G_sentry.friction_motor[SentryRobot::UP_FRICTION_MOTOR]->m_smc->m_fpFB;
    
    //G_vofa.m_data_send_frame.m_data[19] = G_sentry.friction_motor[SentryRobot::DOWN_FRICTION_MOTOR]->m_smc->m_fpFB;
    G_vofa.m_data_send_frame.m_data[19]=G_system_monitor.UART4_rx_fps;
}


//TODO:【发射】数据可视化，发到VOFA上进行观测
/**
 *@brief visualize shoot datas
 *
 *@param
 */
void VisualizeShootData(void)
{
    G_vofa.m_data_send_frame.m_data[0] = G_sentry.friction_motor[SentryRobot::UP_FRICTION_MOTOR]->m_smc->m_fpDes;
    G_vofa.m_data_send_frame.m_data[1] = G_sentry.friction_motor[SentryRobot::UP_FRICTION_MOTOR]->m_smc->m_fpFB;
    G_vofa.m_data_send_frame.m_data[2] = G_sentry.friction_motor[SentryRobot::DOWN_FRICTION_MOTOR]->m_smc->m_fpDes;
    G_vofa.m_data_send_frame.m_data[3] = G_sentry.friction_motor[SentryRobot::DOWN_FRICTION_MOTOR]->m_smc->m_fpFB;

    G_vofa.m_data_send_frame.m_data[4] = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_target;
    G_vofa.m_data_send_frame.m_data[5] = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_current;

    G_vofa.m_data_send_frame.m_data[6] = G_sentry.action_time;
    G_vofa.m_data_send_frame.m_data[7] = G_sentry.shoot_count;
    G_vofa.m_data_send_frame.m_data[8] = G_sentry.m_shoot_speed_filter->m_avg;

    G_vofa.m_data_send_frame.m_data[9] = G_sentry.m_shoot_bullet_fps;
    G_vofa.m_data_send_frame.m_data[10] = G_sentry.shoot_insurance;
    G_vofa.m_data_send_frame.m_data[11] = G_sentry.shoot_driver_angle_target;

    G_vofa.m_data_send_frame.m_data[12] = G_aim_assist.m_data_receive_frame.m_data_f[2];
    G_vofa.m_data_send_frame.m_data[13] = G_sentry.shoot_cool_flag;
    G_vofa.m_data_send_frame.m_data[14] = G_sentry.enemy_attackable_flag;
    G_vofa.m_data_send_frame.m_data[15] = G_sentry.m_shoot_bullet_cnt_target;
    G_vofa.m_data_send_frame.m_data[16] = G_sentry.enemy_track_flag;
    G_vofa.m_data_send_frame.m_data[17] = G_sentry.enemy_find_flag;
}



/**
 *@brief visualize shoot datas
 *
 *@param
 */
void VisualizeAimAssistData(void) 
{
    
}



/**
 *@brief visualize refree datas
 *
 *@param
 */
void VisualizeRefreeData(void) 
{
   
}


//TODO:【裁判】数据可视化，发到VOFA上进行观测
/**
 *@brief visualize refree datas
 *
 *@param
 */
void VisualizeTestData(void) 
{
    G_vofa.m_data_send_frame.m_data[0] = G_system_monitor.CAN1_rx_fps;
    G_vofa.m_data_send_frame.m_data[1] = G_system_monitor.CAN2_rx_fps;
    G_vofa.m_data_send_frame.m_data[2] = G_system_monitor.Gimbal_CAN1_fps;
    G_vofa.m_data_send_frame.m_data[3] = G_system_monitor.Gimbal_CAN2_fps;

    G_vofa.m_data_send_frame.m_data[4] = G_referee.RobotHurt.hurt_type;
    G_vofa.m_data_send_frame.m_data[5] = G_referee.RobotHurt.armor_id;
    G_vofa.m_data_send_frame.m_data[6] = G_referee.GameRobotStatus.remain_HP;

    G_vofa.m_data_send_frame.m_data[7] = G_referee.PowerHeatData.chassis_power_buffer;
    G_vofa.m_data_send_frame.m_data[8] = G_referee.PowerHeatData.chassis_power;
    G_vofa.m_data_send_frame.m_data[9] = G_sentry.capacitor->m_power_charging;
    G_vofa.m_data_send_frame.m_data[10] = G_sentry.capacitor->m_pow_in;
    G_vofa.m_data_send_frame.m_data[11] = G_system_monitor.Aimassist_Receive_fps;
    G_vofa.m_data_send_frame.m_data[12] =G_referee.GameRobotStatus.shooter_barrel_cooling_value;
    G_vofa.m_data_send_frame.m_data[13] =G_referee.GameRobotStatus.shooter_barrel_heat_limit;

    G_vofa.m_data_send_frame.m_data[14] = G_referee.PowerHeatData.chassis_power;//底盘功率
    G_vofa.m_data_send_frame.m_data[15] = G_referee.GameRobotStatus.chassis_power_limit;//底盘功率上限
    G_vofa.m_data_send_frame.m_data[16] = G_referee.PowerHeatData.chassis_power_buffer;//底盘缓冲能
    G_vofa.m_data_send_frame.m_data[17] =G_system_monitor.Referee_Receive_fps;//裁判系统帧率
    G_vofa.m_data_send_frame.m_data[18] = G_sentry.capacitor->m_power_charging;//发给超电的充电功率
    G_vofa.m_data_send_frame.m_data[19]= G_system_monitor.UART2_rx_fps;
}


//TODO:【系统】比赛信息可视化，发到VOFA上进行观测
/**
 *@brief visualize race datas
 *
 *@param
 */
void VisualizeRaceData(void) 
{
    G_vofa.m_data_send_frame_whole.m_data[0] = G_system_monitor.SysTickTime;
    G_vofa.m_data_send_frame_whole.m_data[1] = (float) *((int *) &G_system_monitor.SysFault);
    // chassis
    // 电机响应
    G_vofa.m_data_send_frame_whole.m_data[2] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FL_MOTOR]->m_speed_target;
    G_vofa.m_data_send_frame_whole.m_data[3] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FL_MOTOR]->m_speed_current;
    G_vofa.m_data_send_frame_whole.m_data[4] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FR_MOTOR]->m_speed_target;
    G_vofa.m_data_send_frame_whole.m_data[5] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FR_MOTOR]->m_speed_current;
    G_vofa.m_data_send_frame_whole.m_data[6] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BL_MOTOR]->m_speed_target;
    G_vofa.m_data_send_frame_whole.m_data[7] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BL_MOTOR]->m_speed_current;
    G_vofa.m_data_send_frame_whole.m_data[8] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BR_MOTOR]->m_speed_target;
    G_vofa.m_data_send_frame_whole.m_data[9] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BR_MOTOR]->m_speed_current;
    G_vofa.m_data_send_frame_whole.m_data[10] = G_sentry.chassis_steer_motor[SentryRobot::CHASSIS_S_BL_MOTOR]->m_angle_target;
    G_vofa.m_data_send_frame_whole.m_data[11] = G_sentry.chassis_steer_motor[SentryRobot::CHASSIS_S_BL_MOTOR]->m_angle_current;
    G_vofa.m_data_send_frame_whole.m_data[12] = G_sentry.chassis_steer_motor[SentryRobot::CHASSIS_S_BR_MOTOR]->m_angle_target;
    G_vofa.m_data_send_frame_whole.m_data[13] = G_sentry.chassis_steer_motor[SentryRobot::CHASSIS_S_BR_MOTOR]->m_angle_current;
    // 整体响应
    G_vofa.m_data_send_frame_whole.m_data[14] = G_sentry.m_chassis2world_angle_target;
    G_vofa.m_data_send_frame_whole.m_data[15] = G_sentry.m_chassis2world_angle_current;
    G_vofa.m_data_send_frame_whole.m_data[16] = G_sentry.m_chassis_spin_speed_target;
    G_vofa.m_data_send_frame_whole.m_data[17] = G_sentry.m_chassis_spin_speed;
    G_vofa.m_data_send_frame_whole.m_data[18] = G_sentry.m_chassis_speed_target;
    G_vofa.m_data_send_frame_whole.m_data[19] = G_sentry.m_chassis_speed;
    G_vofa.m_data_send_frame_whole.m_data[20] = G_sentry.m_chassis_speed2radar_angle_target;
    G_vofa.m_data_send_frame_whole.m_data[21] = G_navigation.m_data_send_frame.m_data[3];
    // 角度反馈
    G_vofa.m_data_send_frame_whole.m_data[22] = G_sentry.m_robot_radar_world_yaw;
    G_vofa.m_data_send_frame_whole.m_data[23] = G_sentry.m_robot_radar_world_yaw_filter;
    G_vofa.m_data_send_frame_whole.m_data[24] = G_sentry.m_robot_radar_world_yaw - G_sentry.m_chassis2world_angle_current;
    // 超电功率
    G_vofa.m_data_send_frame_whole.m_data[25] = G_sentry.capacitor->m_cap_vol;
    G_vofa.m_data_send_frame_whole.m_data[26] = G_sentry.capacitor->m_volt_out;
    G_vofa.m_data_send_frame_whole.m_data[27] = G_sentry.capacitor->m_pow_out;
    // gimbal
    // 电机响应
    G_vofa.m_data_send_frame_whole.m_data[28] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_target;
    G_vofa.m_data_send_frame_whole.m_data[29] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current;
    G_vofa.m_data_send_frame_whole.m_data[30] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current_encoder;

    G_vofa.m_data_send_frame_whole.m_data[31] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_target;
    G_vofa.m_data_send_frame_whole.m_data[32] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current;
    G_vofa.m_data_send_frame_whole.m_data[33] = G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current_encoder;

    G_vofa.m_data_send_frame_whole.m_data[34] = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_target;
    G_vofa.m_data_send_frame_whole.m_data[35] = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current;
    G_vofa.m_data_send_frame_whole.m_data[36] = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current_encoder;

    G_vofa.m_data_send_frame_whole.m_data[37] = G_sentry.enemy_find_flag;
    // shoot
    // 电机响应
    G_vofa.m_data_send_frame_whole.m_data[38] = G_sentry.friction_motor[SentryRobot::UP_FRICTION_MOTOR]->m_smc->m_fpDes;
    G_vofa.m_data_send_frame_whole.m_data[39] = G_sentry.friction_motor[SentryRobot::UP_FRICTION_MOTOR]->m_smc->m_fpFB;
    G_vofa.m_data_send_frame_whole.m_data[40] = G_sentry.friction_motor[SentryRobot::DOWN_FRICTION_MOTOR]->m_smc->m_fpDes;
    G_vofa.m_data_send_frame_whole.m_data[41] = G_sentry.friction_motor[SentryRobot::DOWN_FRICTION_MOTOR]->m_smc->m_fpFB;

    G_vofa.m_data_send_frame_whole.m_data[42] = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_target;
    G_vofa.m_data_send_frame_whole.m_data[43] = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_current;

    G_vofa.m_data_send_frame_whole.m_data[44] = G_aim_assist.m_data_receive_frame.m_data_f[2];
    G_vofa.m_data_send_frame_whole.m_data[45] = G_sentry.shoot_cool_flag;
    G_vofa.m_data_send_frame_whole.m_data[46] = G_sentry.enemy_attackable_flag;
    G_vofa.m_data_send_frame_whole.m_data[47] = G_sentry.enemy_track_flag;    
    G_vofa.m_data_send_frame_whole.m_data[48] = G_sentry.m_shoot_bullet_cnt_target;

}


//TODO：【裁判】数据
/**
 *@brief send referee datas
 *
 *@param
 */
void SendRefereeData(void)
{

    uint8_t mapdata_send_flag,decisiondata_send_flag,radar_referee_send_flag,radar_navigation_flag,ui_send_flag;
    static uint8_t cnt;
    cnt++;
    if(cnt>100-1)
    cnt = 0;

    //分频发送
    mapdata_send_flag       = (cnt==0);    //每100一次
    decisiondata_send_flag  = (cnt%10==3); //每10一次
    radar_referee_send_flag = (cnt==50);   //每50一次
    radar_navigation_flag   = (cnt%20==16);//每20一次
    ui_send_flag = (cnt==50);


    //地图
#ifdef CHASSIS_STEER_DRIVING_MODE
    if(G_sentry.m_robot_chassis_spin_ready){
        G_referee.MapSentryData.intention = 2; // SPIN
    }else if(!G_sentry.m_robot_chassis_spin_ready && G_sentry.m_chassis_speed > CHASSIS_SPIN_OVER_LIMIT){
        G_referee.MapSentryData.intention = 1; // MOVE
    }else{
        G_referee.MapSentryData.intention = 3; // STOP
    }
#endif

#ifdef CHASSIS_STANDARD_DRIVING_MODE
    G_referee.MapSentryData.intention = 3; // STOP
#endif

    G_referee.MapSentryData.intention = 3;
    uint16_t current_pos_x = G_navigation.m_data_receive_frame.m_data_f[7]*10;
    uint16_t current_pos_y = G_navigation.m_data_receive_frame.m_data_f[8]*10;
    uint16_t current_target_x = G_navigation.m_data_receive_frame.m_data_f[9]*10;
    uint16_t current_target_y = G_navigation.m_data_receive_frame.m_data_f[10]*10;
    //初始化，在把目标位置到当前位置的增量全都置零后，初始化位置标志位置真，然后才开始更新delta_x和delta_y
    if(!G_referee.m_loc_send_init_flag)
    {
        for(uint8_t i = 0; i < REFEREE_MAP_SIZE; i++){
            *(G_referee.MapSentryData.delta_x+i) = 0;
            *(G_referee.MapSentryData.delta_y+i) = 0;
        }
        G_referee.m_loc_send_init_flag = true;
    }
    G_referee.MapSentryData.delta_x[0] = current_target_x - current_pos_x;
    G_referee.MapSentryData.delta_y[0] = current_target_y - current_pos_y;
    G_referee.MapSentryData.start_position_x = current_pos_x;
    G_referee.MapSentryData.start_position_y = current_pos_y;
    G_referee.MapSentryData.sender_id = G_referee.GameRobotStatus.robot_id;//发送者哨兵id


    //雷达数据
    if(radar_navigation_flag)//每20一次
    {
        G_referee.RadarEnemyData.data_cmd_id = 0x207;
        G_referee.RadarEnemyData.sender_id = G_referee.GameRobotStatus.robot_id;
        G_referee.RadarEnemyData.receiver_id = G_referee.GameRobotStatus.robot_id+2;//雷达、烧饼ID相差2
        G_referee.RadarEnemyData.data.label_enemy_1 = G_navigation.m_data_receive_frame.m_data_c[3];
        G_referee.RadarEnemyData.data.label_enemy_2 = G_navigation.m_data_receive_frame.m_data_c[4];
        G_referee.RadarEnemyData.data.label_enemy_3 = G_navigation.m_data_receive_frame.m_data_c[5];
        G_referee.RadarEnemyData.data.label_enemy_4 = G_navigation.m_data_receive_frame.m_data_c[6];
        G_referee.RadarEnemyData.data.x_enemy_1 = G_navigation.m_data_receive_frame.m_data_f[11];
        G_referee.RadarEnemyData.data.y_enemy_1 = G_navigation.m_data_receive_frame.m_data_f[12];
        G_referee.RadarEnemyData.data.x_enemy_2 = G_navigation.m_data_receive_frame.m_data_f[13];
        G_referee.RadarEnemyData.data.y_enemy_2 = G_navigation.m_data_receive_frame.m_data_f[14];
        G_referee.RadarEnemyData.data.x_enemy_3 = G_navigation.m_data_receive_frame.m_data_f[15];
        G_referee.RadarEnemyData.data.y_enemy_3 = G_navigation.m_data_receive_frame.m_data_f[16];
        G_referee.RadarEnemyData.data.x_enemy_4 = G_navigation.m_data_receive_frame.m_data_f[17];
        G_referee.RadarEnemyData.data.y_enemy_4 = G_navigation.m_data_receive_frame.m_data_f[18];

        uint16_t data_length;
        uint16_t cmd_id;

        data_length = RADAR_DATA_BITS_LENGTH;
        cmd_id = RADAR_CMD_ID;

        //数据长度
        G_referee.m_data_send_buff[1] = (uint8_t) data_length;
        G_referee.m_data_send_buff[2] = (uint8_t) (data_length >> 8);
        Append_CRC8_Check_Sum(G_referee.m_data_send_buff,5);
        //命令码
        G_referee.m_data_send_buff[5] = (uint8_t) cmd_id;
        G_referee.m_data_send_buff[6] = (uint8_t) (cmd_id >> 8);
        memcpy(&G_referee.m_data_send_buff[7], &G_referee.RadarEnemyData, data_length);

        //发送
        Append_CRC16_Check_Sum(G_referee.m_data_send_buff, data_length + NONDATA_BITS_LENGTH);//TODO:【通讯】CRC校验码的具体实践？
        G_referee.SendData(data_length + NONDATA_BITS_LENGTH);
    }


    else if(radar_referee_send_flag)//每50一次
    {
        G_referee.RadarSelfData.data_cmd_id = 0x277;
        G_referee.RadarSelfData.sender_id = G_referee.GameRobotStatus.robot_id;
        G_referee.RadarSelfData.receiver_id = G_referee.GameRobotStatus.robot_id+2;

        memcpy(&G_referee.RadarSelfData.data, &G_referee.GroundRobotPosition, sizeof(G_referee.GroundRobotPosition));
        G_referee.RadarSelfData.data.x_sentry = G_navigation.m_data_receive_frame.m_data_f[7];
        G_referee.RadarSelfData.data.y_sentry = G_navigation.m_data_receive_frame.m_data_f[8];


        uint16_t data_length;
        uint16_t cmd_id;

        data_length = RADAR_DATA_BITS_LENGTH;
        cmd_id = RADAR_CMD_ID;

        //数据长度
        G_referee.m_data_send_buff[1] = (uint8_t) data_length;
        G_referee.m_data_send_buff[2] = (uint8_t) (data_length >> 8);
        Append_CRC8_Check_Sum(G_referee.m_data_send_buff,5);
        //命令码
        G_referee.m_data_send_buff[5] = (uint8_t) cmd_id;
        G_referee.m_data_send_buff[6] = (uint8_t) (cmd_id >> 8);
        memcpy(&G_referee.m_data_send_buff[7], &G_referee.RadarSelfData, data_length);

        //发送
        Append_CRC16_Check_Sum(G_referee.m_data_send_buff, data_length + NONDATA_BITS_LENGTH);
        G_referee.SendData(data_length + NONDATA_BITS_LENGTH);

    }


    else if(mapdata_send_flag)//每100一次
    {
        uint16_t data_length;
        uint16_t cmd_id;

        data_length = MAP_DATA_BITS_LENGTH;
        // data_length = sizeof(G_referee.MapSentryData);
        cmd_id = MAP_CMD_ID;

        //数据长度
        G_referee.m_data_send_buff[1] = (uint8_t) data_length;
        G_referee.m_data_send_buff[2] = (uint8_t) (data_length >> 8);
        Append_CRC8_Check_Sum(G_referee.m_data_send_buff,5);
        //命令码
        G_referee.m_data_send_buff[5] = (uint8_t) cmd_id;
        G_referee.m_data_send_buff[6] = (uint8_t) (cmd_id >> 8);
        memcpy(&G_referee.m_data_send_buff[7], &G_referee.MapSentryData, data_length);

        //发送
        Append_CRC16_Check_Sum(G_referee.m_data_send_buff, data_length + NONDATA_BITS_LENGTH);
        G_referee.SendData(data_length + NONDATA_BITS_LENGTH);
    }


    else if(decisiondata_send_flag)//每10一次
    {
        //决策数据
        G_referee.SentryDecisionData.data_cmd_id = 0x120;
        G_referee.SentryDecisionData.sender_id = G_referee.GameRobotStatus.robot_id;
        G_referee.SentryDecisionData.receiver_id = (uint16_t)0x8080;

        if(G_referee.GameRobotStatus.remain_HP == 0)
            G_referee.SentryDecisionData.confirm_resurrect_flag = 1;
        else    
            G_referee.SentryDecisionData.confirm_resurrect_flag = 0;
        
        G_referee.SentryDecisionData.exchange_bullet_capacity = 0;
        G_referee.SentryDecisionData.remote_exchange_bullet_capacity_cnt = 0;
        
        uint16_t data_length;
        uint16_t cmd_id;

        data_length = DECISION_DATA_BITS_LENGTH;
        cmd_id = DECISION_CMD_ID;

        //数据长度
        G_referee.m_data_send_buff[1] = (uint8_t) data_length;
        G_referee.m_data_send_buff[2] = (uint8_t) (data_length >> 8);
        Append_CRC8_Check_Sum(G_referee.m_data_send_buff,5);
        //命令码
        G_referee.m_data_send_buff[5] = (uint8_t) cmd_id;
        G_referee.m_data_send_buff[6] = (uint8_t) (cmd_id >> 8);
        memcpy(&G_referee.m_data_send_buff[7], &G_referee.SentryDecisionData, data_length);

        //发送
        Append_CRC16_Check_Sum(G_referee.m_data_send_buff, data_length + NONDATA_BITS_LENGTH);
        G_referee.SendData(data_length + NONDATA_BITS_LENGTH);
    }
        /*裁判系统UI绘制*/
    else 
    {
        G_sentry.New_Client();  //客户端新建图层
    }



}
//TODO:【注意】目标值都是相对于imu的目标值


//TODO:【决策】?导航信息发送 que：坐标转换、雷达与云台的相对位置
/**
 *@brief send navigation datas
 *
 *@param
 *
 * @note 
 */
void SendNavigationData(void)
{
    //G_navigation.m_data_send_frame.m_data[2] = G_sentry.m_chassis_speed;
    //G_navigation.m_data_send_frame.m_data[8] = G_sentry.capacitor->m_pow_out;
    //G_navigation.m_data_send_frame.m_data[9] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FL_MOTOR]->m_speed_current;
    //G_navigation.m_data_send_frame.m_data[10] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FR_MOTOR]->m_speed_current;
    //G_navigation.m_data_send_frame.m_data[11] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BL_MOTOR]->m_speed_current;
    //G_navigation.m_data_send_frame.m_data[12] = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BR_MOTOR]->m_speed_current;
    //G_navigation.m_data_send_frame.m_data[13] = G_gimbal.m_data_receive_frame.m_fdata[4];
    G_navigation.m_data_send_frame.m_data[2] = G_sentry.m_chassis_speed;
    G_navigation.m_data_send_frame.m_data[8] = G_referee.PowerHeatData.chassis_power;
    G_navigation.m_data_send_frame.m_data[9] = G_sentry.capacitor->m_pow_out;
    G_navigation.m_data_send_frame.m_data[10] = float(G_sentry.stop_flag);
    G_navigation.m_data_send_frame.m_data[11] = G_referee.PowerHeatData.chassis_power_buffer;
    

    if(G_navigation.m_data_receive_frame.m_data_c[1] == 0 || G_navigation.m_data_receive_frame.m_data_c[1] == 1)
        G_navigation.m_data_send_frame.m_data[3] = RangeFmod(G_sentry.m_robot_radar_world_yaw_abs,-180,180); 
    else if(G_navigation.m_data_receive_frame.m_data_c[1] == 2 || G_navigation.m_data_receive_frame.m_data_c[1] == 3)
        G_navigation.m_data_send_frame.m_data[3] = RangeFmod(G_sentry.m_chassis_speed2chassis_angle + G_sentry.m_robot_radar_world_yaw_abs,-180,180); // warning
    
    if(G_sentry.enemy_find_flag)//如果发现敌人，辅瞄数据计算，数据从导航到辅瞄的转换
    {
        //从导航坐标系到辅瞄坐标系需要旋转的角度
        float navigation2aimassist_angle = (G_sentry.m_robot_radar_world_yaw_filter+G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current_encoder-G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current)/RADIAN2DEGREE_VALUE;
        float gimbal_dev = 0.06;//坐标系转换修正
        float gimbal_high = 0.125;//坐标系转换修正
        //辅瞄得到的相对位置
        float aimassist_x = G_aim_assist.m_data_receive_frame.m_data_f[4];//向右 敌人的坐标相对车的坐标
        float aimassist_y = G_aim_assist.m_data_receive_frame.m_data_f[5];//向下
        float aimassist_z = G_aim_assist.m_data_receive_frame.m_data_f[6];//向前
        //转换到导航坐标系下
        float radar_x = (aimassist_z) * cos(navigation2aimassist_angle) + aimassist_x * sin(navigation2aimassist_angle);//向前
        float radar_y = - aimassist_x * cos(navigation2aimassist_angle) + (aimassist_z) * sin(navigation2aimassist_angle);//向左
        float radar_z = - aimassist_y + gimbal_high;//向上
        G_navigation.m_data_send_frame.m_data[4] = G_aim_assist.m_data_receive_frame.m_data_f[3];
        G_navigation.m_data_send_frame.m_data[5] = radar_x;
        G_navigation.m_data_send_frame.m_data[6] = radar_y;
        G_navigation.m_data_send_frame.m_data[7] = radar_z;
        G_sentry.test_box1 = navigation2aimassist_angle*RADIAN2DEGREE_VALUE;
    }
    else
    {
        G_navigation.m_data_send_frame.m_data[4] = 0;
        G_navigation.m_data_send_frame.m_data[5] = 0;
        G_navigation.m_data_send_frame.m_data[6] = 0;
        G_navigation.m_data_send_frame.m_data[7] = 0;
	    G_navigation.m_data_send_frame.m_data[8] = 0;
        G_navigation.m_data_send_frame.m_data[8] = G_sentry.capacitor->m_pow_out;
    }

//将导航数据信息存入一个缓冲区发出去
    uint32_t navigation_data_num = sizeof(G_navigation.m_data_send_frame);
    if (G_navigation.navigation_send_buff_cnt + navigation_data_num + NAVIGATION_SEND_HEAD_SIZE < NAVIGATION_SEND_BUFF_SIZE)
    {
        G_navigation.navigation_send_buff[G_navigation.navigation_send_buff_cnt++] = 0xAB;
        G_navigation.navigation_send_buff[G_navigation.navigation_send_buff_cnt++] = 0xA0;
        G_navigation.navigation_send_buff[G_navigation.navigation_send_buff_cnt++] = navigation_data_num;
        memcpy(&G_navigation.navigation_send_buff[G_navigation.navigation_send_buff_cnt], &G_navigation.m_data_send_frame, navigation_data_num);
        G_navigation.navigation_send_buff_cnt += navigation_data_num;
    }
    G_navigation.usart_navigation->USART_RT.txlen = G_navigation.navigation_send_buff_cnt;
    G_navigation.SendData();
    G_navigation.navigation_send_buff_cnt = 0;

    G_navigation.m_data_send_frame.m_id = 0x00;
}


//TODO:【决策】辅瞄信息发送以及发射决策
/**
 *@brief send AimAssist datas
 *
 *@param
 */
void SendAimAssistData(void) {
    if (G_referee.GameRobotStatus.robot_id == ROBOT_ID_SENTRY_RED) {
        // 這個判斷放這裏好像不太對
        G_aim_assist.m_data_send_frame.m_id = 0x09;
        //敌人是否无敌
        if(G_referee.m_enemy_1_HP_pre == 0 && G_referee.GameRobotHP.blue_1_robot_HP != 0){
            G_aim_assist.m_enemy_1_res_flag = true;
        }
        if(G_referee.m_enemy_2_HP_pre == 0 && G_referee.GameRobotHP.blue_2_robot_HP != 0){
            G_aim_assist.m_enemy_2_res_flag = true;
        }
        if(G_referee.m_enemy_3_HP_pre == 0 && G_referee.GameRobotHP.blue_3_robot_HP != 0){
            G_aim_assist.m_enemy_3_res_flag = true;
        }
        if(G_referee.m_enemy_4_HP_pre == 0 && G_referee.GameRobotHP.blue_4_robot_HP != 0){
            G_aim_assist.m_enemy_4_res_flag = true;
        }
        if(G_referee.m_enemy_5_HP_pre == 0 && G_referee.GameRobotHP.blue_5_robot_HP != 0){
            G_aim_assist.m_enemy_5_res_flag = true;
        }
        if(G_referee.GameRobotHP.blue_outpost_HP == 0){
            G_aim_assist.m_enemy_7_res_flag = false;
        }else{
            G_aim_assist.m_enemy_7_res_flag = true;
        }
        //敌人无敌时间记录
        if(G_aim_assist.m_enemy_1_res_flag == true && G_aim_assist.m_enemy_1_res_cnt <= 10000){
            G_aim_assist.m_enemy_1_res_cnt++;
        }else{
            G_aim_assist.m_enemy_1_res_flag = false;
            G_aim_assist.m_enemy_1_res_cnt = 0;
        }
        if(G_aim_assist.m_enemy_2_res_flag == true && G_aim_assist.m_enemy_2_res_cnt <= 10000){
            G_aim_assist.m_enemy_2_res_cnt++;
        }else{
            G_aim_assist.m_enemy_2_res_flag = false;
            G_aim_assist.m_enemy_2_res_cnt = 0;
        }
        if(G_aim_assist.m_enemy_3_res_flag == true && G_aim_assist.m_enemy_3_res_cnt <= 10000){
            G_aim_assist.m_enemy_3_res_cnt++;
        }else{
            G_aim_assist.m_enemy_3_res_flag = false;
            G_aim_assist.m_enemy_3_res_cnt = 0;
        }
        if(G_aim_assist.m_enemy_4_res_flag == true && G_aim_assist.m_enemy_4_res_cnt <= 10000){
            G_aim_assist.m_enemy_4_res_cnt++;
        }else{
            G_aim_assist.m_enemy_4_res_flag = false;
            G_aim_assist.m_enemy_4_res_cnt = 0;
        }
        if(G_aim_assist.m_enemy_5_res_flag == true && G_aim_assist.m_enemy_5_res_cnt <= 10000){
            G_aim_assist.m_enemy_5_res_cnt++;
        }else{
            G_aim_assist.m_enemy_5_res_flag = false;
            G_aim_assist.m_enemy_5_res_cnt = 0;
        }
        //TODO:【决策】？ enemy_vul_rig包含这个定义的结构体里定义的含义
        G_aim_assist.enmey_states.enemy_vul_rig = 0xff;
        if(G_aim_assist.m_enemy_7_res_flag)
            G_aim_assist.enmey_states.enemy_vul_rig = G_aim_assist.enmey_states.enemy_vul_rig & 0b11111101;
        G_aim_assist.enmey_states.enemy_rec_rig = 0x00;
        G_aim_assist.enmey_states.Hero_HP = G_referee.GameRobotHP.blue_1_robot_HP;
        G_aim_assist.enmey_states.Infantry3_HP = G_referee.GameRobotHP.blue_3_robot_HP;
        G_aim_assist.enmey_states.Infantry4_HP = G_referee.GameRobotHP.blue_4_robot_HP;
        G_aim_assist.enmey_states.EngineerHP = G_referee.GameRobotHP.blue_2_robot_HP;


    } else {//我方为蓝方
        G_aim_assist.m_data_send_frame.m_id = 0x0A;
        if(G_referee.m_enemy_1_HP_pre == 0 && G_referee.GameRobotHP.red_1_robot_HP != 0){
            G_aim_assist.m_enemy_1_res_flag = true;
        }
        if(G_referee.m_enemy_2_HP_pre == 0 && G_referee.GameRobotHP.red_2_robot_HP != 0){
            G_aim_assist.m_enemy_2_res_flag = true;
        }
        if(G_referee.m_enemy_3_HP_pre == 0 && G_referee.GameRobotHP.red_3_robot_HP != 0){
            G_aim_assist.m_enemy_3_res_flag = true;
        }
        if(G_referee.m_enemy_4_HP_pre == 0 && G_referee.GameRobotHP.red_4_robot_HP != 0){
            G_aim_assist.m_enemy_4_res_flag = true;
        }
        if(G_referee.m_enemy_5_HP_pre == 0 && G_referee.GameRobotHP.red_5_robot_HP != 0){
            G_aim_assist.m_enemy_5_res_flag = true;
        }
        if(G_referee.GameRobotHP.red_outpost_HP == 0){
            G_aim_assist.m_enemy_7_res_flag = false;
        }else{
            G_aim_assist.m_enemy_7_res_flag = true;
        }

        if(G_aim_assist.m_enemy_1_res_flag == true && G_aim_assist.m_enemy_1_res_cnt <= 10000){
            G_aim_assist.m_enemy_1_res_cnt++;
        }else{
            G_aim_assist.m_enemy_1_res_flag = false;
            G_aim_assist.m_enemy_1_res_cnt = 0;
        }
        if(G_aim_assist.m_enemy_2_res_flag == true && G_aim_assist.m_enemy_2_res_cnt <= 10000){
            G_aim_assist.m_enemy_2_res_cnt++;
        }else{
            G_aim_assist.m_enemy_2_res_flag = false;
            G_aim_assist.m_enemy_2_res_cnt = 0;
        }
        if(G_aim_assist.m_enemy_3_res_flag == true && G_aim_assist.m_enemy_3_res_cnt <= 10000){
            G_aim_assist.m_enemy_3_res_cnt++;
        }else{
            G_aim_assist.m_enemy_3_res_flag = false;
            G_aim_assist.m_enemy_3_res_cnt = 0;
        }
        if(G_aim_assist.m_enemy_4_res_flag == true && G_aim_assist.m_enemy_4_res_cnt <= 10000){
            G_aim_assist.m_enemy_4_res_cnt++;
        }else{
            G_aim_assist.m_enemy_4_res_flag = false;
            G_aim_assist.m_enemy_4_res_cnt = 0;
        }
        if(G_aim_assist.m_enemy_5_res_flag == true && G_aim_assist.m_enemy_5_res_cnt <= 10000){
            G_aim_assist.m_enemy_5_res_cnt++;
        }else{
            G_aim_assist.m_enemy_5_res_flag = false;
            G_aim_assist.m_enemy_5_res_cnt = 0;
        }
        

        G_aim_assist.enmey_states.enemy_vul_rig = 0xff;
        if(G_aim_assist.m_enemy_7_res_flag)
            G_aim_assist.enmey_states.enemy_vul_rig = G_aim_assist.enmey_states.enemy_vul_rig & 0b11111101;
        G_aim_assist.enmey_states.enemy_rec_rig = 0x00;
        G_aim_assist.enmey_states.Hero_HP = G_referee.GameRobotHP.red_1_robot_HP;
        G_aim_assist.enmey_states.Infantry3_HP = G_referee.GameRobotHP.red_3_robot_HP;
        G_aim_assist.enmey_states.Infantry4_HP = G_referee.GameRobotHP.red_4_robot_HP;
        G_aim_assist.enmey_states.EngineerHP  = G_referee.GameRobotHP.red_2_robot_HP;
    }
    
    G_aim_assist.m_data_send_frame.m_data[0] = 
    G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current;
    G_aim_assist.m_data_send_frame.m_data[1] = 
    G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current;
    G_aim_assist.m_data_send_frame.m_data[2] = G_sentry.gimbal_roll;
    G_aim_assist.m_data_send_frame.m_data[3] = G_sentry.m_shoot_speed_filter->m_avg;

    if ( G_system_monitor.Navigation_Receive_fps < 5) {//掉线了 寄！
        G_aim_assist.m_data_send_frame.m_data[4] = 0;//x
        G_aim_assist.m_data_send_frame.m_data[5] = 0;//y
        G_aim_assist.m_data_send_frame.m_data[6] = 0;//z
        G_aim_assist.m_data_send_frame.m_data[7] = -1;//blood
        G_aim_assist.m_data_send_frame.m_data[8] = 8;//num
        G_aim_assist.m_data_send_frame.m_data[9] = 0;//x pos
        G_aim_assist.m_data_send_frame.m_data[10] = 0;//y pos
        G_aim_assist.m_data_send_frame.m_data[11] = 0;// error angle

    } else {//TODO: que:视觉那边发过来的数据的结构
        float error_angle = (G_sentry.m_robot_radar_world_yaw_filter+G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current_encoder-G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current)/RADIAN2DEGREE_VALUE;
        float gimbal_dev = 0.06;
        float gimbal_high = 0.125;
        float radar_x = G_navigation.m_data_receive_frame.m_data_f[3];//向前
        float radar_y = G_navigation.m_data_receive_frame.m_data_f[4];//向左
        float radar_z = G_navigation.m_data_receive_frame.m_data_f[5];//向上
        float aimassist_x = -radar_y*cos(error_angle)+radar_x*sin(error_angle);//向右
        float aimassist_y = gimbal_high-G_navigation.m_data_receive_frame.m_data_f[5];//向下
        float aimassist_z = radar_x*cos(error_angle)+radar_y*sin(error_angle) - gimbal_dev;//向前
        G_aim_assist.m_data_send_frame.m_data[4] = aimassist_x;
        G_aim_assist.m_data_send_frame.m_data[5] = aimassist_y;
        G_aim_assist.m_data_send_frame.m_data[6] = aimassist_z;
        G_aim_assist.m_data_send_frame.m_data[7] = G_navigation.m_data_receive_frame.m_data_f[6];
        G_aim_assist.m_data_send_frame.m_data[8] = G_navigation.m_data_receive_frame.m_data_c[0];
        G_aim_assist.m_data_send_frame.m_data[9] = G_navigation.m_data_receive_frame.m_data_f[7];
        G_aim_assist.m_data_send_frame.m_data[10] = -G_navigation.m_data_receive_frame.m_data_f[8];
        G_aim_assist.m_data_send_frame.m_data[11] = -error_angle;
    } 
    
    G_aim_assist.m_data_send_frame.m_data[12] = G_sentry.action_time;
    memcpy(&G_aim_assist.m_data_send_frame.m_data[13], &G_aim_assist.enmey_states, sizeof(G_aim_assist.enmey_states));

    Append_CRC16_Check_Sum((uint8_t*)&G_aim_assist.m_data_send_frame,AIM_ASSIST_DATA_SEND_SIZE);
    G_aim_assist.SendData();
}

//TODO:【云台】？云台发送数据 计算pitch和yaw的电流输出值并且根据模式（gimbal_mode和ff_mode）决定是否有PID输出和反馈（包括重力前馈和摩擦力前馈）。最后通过串口4发给云控
/**
 *@brief send gimabl datas
 *
 *@param
 */
void SendGimbalData(void)
{
    float pitch_current = 0;
    float yaw_current = 0;
    float pitch_ff = 0;
    float yaw_ff = 0;
    
    if (G_sentry.gimbal_mode != SentryRobot::GIMBAL_SAFE)
    {
        pitch_current = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_speed_pid->m_output;
        yaw_current = G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_speed_pid->m_output;
    } 
    if(G_sentry.ff_mode != SentryRobot::NoFF && !(G_sentry.ff_mode == SentryRobot::ControlFF && G_sentry.gimbal_mode == SentryRobot::GIMBAL_SAFE))//有前馈并且（前馈控制模式不是控制前馈或者是安全模式）即安全模式下的任何前馈以及一直有前馈
    {
        pitch_ff = G_sentry.gimbal_pitch_ff_current;
        yaw_ff = G_sentry.gimbal_yaw_ff_current;
    }
    float pitch_out = Clip(pitch_current + pitch_ff, -G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_speed_pid->m_output_max, G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_speed_pid->m_output_max);
    float yaw_out = Clip(yaw_current + yaw_ff, -G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_speed_pid->m_output_max, G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_speed_pid->m_output_max);
    
    //云台电机输出发给云控
    G_gimbal.m_data_send_frame.m_sdata[0] = pitch_out;
    G_gimbal.m_data_send_frame.m_sdata[1] = yaw_out;
    G_gimbal.m_data_send_frame.m_sdata[2] = G_sentry.friction_motor[SentryRobot::UP_FRICTION_MOTOR]->m_smc->m_fpU;
    G_gimbal.m_data_send_frame.m_sdata[3] = G_sentry.friction_motor[SentryRobot::DOWN_FRICTION_MOTOR]->m_smc->m_fpU;
    G_gimbal.m_data_send_frame.m_sdata[4] = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_speed_pid->m_output;

    Append_CRC16_Check_Sum((uint8_t*)&G_gimbal.m_data_send_frame,GIMBAL_DATA_SEND_SIZE);
    G_gimbal.SendData();
}

void USBSendAimAssistData(void)
{

  G_navigation.m_data_send_frame.m_data[2] = G_sentry.m_chassis_speed;
   G_navigation.m_data_send_frame.m_data[8] = G_referee.PowerHeatData.chassis_power;
   G_navigation.m_data_send_frame.m_data[10] = float(G_sentry.stop_flag);
   G_navigation.m_data_send_frame.m_data[11] = G_referee.PowerHeatData.chassis_power_buffer;
   G_navigation.m_data_send_frame.m_data[9] = G_sentry.capacitor->m_pow_out;

   if(G_navigation.m_data_receive_frame.m_data_c[1] == 0 || G_navigation.m_data_receive_frame.m_data_c[1] == 1)
       G_navigation.m_data_send_frame.m_data[3] = RangeFmod(G_sentry.m_robot_radar_world_yaw_abs,-180,180); // warning
   else if(G_navigation.m_data_receive_frame.m_data_c[1] == 2 || G_navigation.m_data_receive_frame.m_data_c[1] == 3)
       G_navigation.m_data_send_frame.m_data[3] = RangeFmod(G_sentry.m_chassis_speed2chassis_angle + G_sentry.m_robot_radar_world_yaw_abs,-180,180); // warning
   
   if(G_sentry.enemy_find_flag)
   {
       float navigation2aimassist_angle = (G_sentry.m_robot_radar_world_yaw_filter+G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current_encoder-G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current)/RADIAN2DEGREE_VALUE;
       float gimbal_dev = 0.06;
       float gimbal_high = 0.125;
       float aimassist_x = G_aim_assist.m_data_receive_frame.m_data_f[4];//向右
       float aimassist_y = G_aim_assist.m_data_receive_frame.m_data_f[5];//向下
       float aimassist_z = G_aim_assist.m_data_receive_frame.m_data_f[6];//向前
       float radar_x = (aimassist_z) * cos(navigation2aimassist_angle) + aimassist_x * sin(navigation2aimassist_angle);//向前
       float radar_y = - aimassist_x * cos(navigation2aimassist_angle) + (aimassist_z) * sin(navigation2aimassist_angle);//向左
       float radar_z = - aimassist_y + gimbal_high;//向上
       G_navigation.m_data_send_frame.m_data[4] = G_aim_assist.m_data_receive_frame.m_data_f[3];
       G_navigation.m_data_send_frame.m_data[5] = radar_x;
       G_navigation.m_data_send_frame.m_data[6] = radar_y;
       G_navigation.m_data_send_frame.m_data[7] = radar_z;
       G_sentry.test_box1 = navigation2aimassist_angle*RADIAN2DEGREE_VALUE;
   }
   else
   {
       G_navigation.m_data_send_frame.m_data[4] = 0;
       G_navigation.m_data_send_frame.m_data[5] = 0;
       G_navigation.m_data_send_frame.m_data[6] = 0;
       G_navigation.m_data_send_frame.m_data[7] = 0;
	    G_navigation.m_data_send_frame.m_data[8] = 0;
       G_navigation.m_data_send_frame.m_data[8] = G_referee.PowerHeatData.chassis_power;

   }


   uint32_t navigation_data_num = sizeof(G_navigation.m_data_send_frame);
   if (G_navigation.navigation_send_buff_cnt + navigation_data_num + NAVIGATION_SEND_HEAD_SIZE < NAVIGATION_SEND_BUFF_SIZE)
   {
       G_navigation.navigation_send_buff[G_navigation.navigation_send_buff_cnt++] = 0xAB;
       G_navigation.navigation_send_buff[G_navigation.navigation_send_buff_cnt++] = 0xA0;
       G_navigation.navigation_send_buff[G_navigation.navigation_send_buff_cnt++] = navigation_data_num;
       memcpy(&G_navigation.navigation_send_buff[G_navigation.navigation_send_buff_cnt], &G_navigation.m_data_send_frame, navigation_data_num);
       G_navigation.navigation_send_buff_cnt += navigation_data_num;
   }
   G_navigation.usart_navigation->USART_RT.txlen = G_navigation.navigation_send_buff_cnt;
   VCP_SendData(&USB_OTG_dev, (u8*)&G_navigation.navigation_send_buff, sizeof(G_navigation.navigation_send_buff));
//    DCD_EP_Tx(&USB_OTG_dev, CDC_IN_EP,(u8*)&G_navigation.navigation_send_buff, sizeof(G_navigation.navigation_send_buff));
   G_navigation.navigation_send_buff_cnt = 0;

   G_navigation.m_data_send_frame.m_id = 0x00;
	//DCD_EP_Tx(&USB_OTG_dev, CDC_IN_EP, buff, sizeof(buff)); 
}


/**
 *@brief update system control mode
 *
 *@param
 */
void ControlModeUpdate(void)
{
    static bool PRESSED_X_pre = false;
    static s32 pressed_time = 0;
    static bool PRESSED_C_pre = false;
    // Control mode update 
    if(G_system_monitor.UART1_rx_fps < 5)//接收不到遥控器信息
    {
        if(G_referee.GameStatus.game_progress == 4)//在对战中
        { 
            static float x_pre, y_pre;
            float press_update_flag = 0;
            if(x_pre != G_referee.RobotCommand.target_position_x || y_pre != G_referee.RobotCommand.target_position_y)
                press_update_flag = 1;
            x_pre = G_referee.RobotCommand.target_position_x;
            y_pre = G_referee.RobotCommand.target_position_y;

            G_control_mode = AUTO_CGS; //比赛中当接收不到遥控器时，保持自动模式

            if(G_referee.RobotCommand.commd_keyboard == PRESS_R && press_update_flag)
                G_referee.safe_cnt = 30000;

            if(G_referee.safe_cnt>0)
                G_referee.safe_cnt --;
            else
                G_referee.safe_cnt = 0;
            
            if(G_referee.safe_cnt > 0)
                G_control_mode = SAFE;

            if(G_referee.GameRobotStatus.remain_HP == 0)
                G_control_mode = SAFE;

            if(G_referee.RobotCommand.commd_keyboard == PRESS_Q)
                G_aim_assist.enmey_states.enemy_attack_flag |= 0x01;
            if(G_referee.RobotCommand.commd_keyboard == PRESS_W)
                G_aim_assist.enmey_states.enemy_attack_flag |= 0x02;
            if(G_referee.RobotCommand.commd_keyboard == PRESS_E)
                G_aim_assist.enmey_states.enemy_attack_flag |= 0x04;

        }
        else
        {
            G_control_mode = SAFE;
        }
    }
//TODO: 【手柄】设置控制模式 左下右中：RC_GS,左下右上：AUTO_G_HALFAUTO_S,左上右上：AUTO_CGS,左右中：RC_GC,其他：SAFE
    else
    {
        if (G_djirc.channel.SW_L == RC_SW_DOWN && G_djirc.channel.SW_R == RC_SW_MID)
        {
            G_control_mode = RC_GS;
        }
        else if (G_djirc.channel.SW_L == RC_SW_DOWN && G_djirc.channel.SW_R == RC_SW_UP)
        {
            G_control_mode = AUTO_G_HALFAUTO_S;
        }
        else if (G_djirc.channel.SW_L == RC_SW_UP && G_djirc.channel.SW_R == RC_SW_UP)
        {
            G_control_mode = AUTO_CGS;
        }
        else if (G_djirc.channel.SW_L == RC_SW_MID && G_djirc.channel.SW_R == RC_SW_MID)
        {
           G_control_mode = RC_GC;
            // G_control_mode = SAFE;

        }
        else if (  G_djirc.channel.SW_L == RC_SW_MID && G_djirc.channel.SW_R == RC_SW_UP )
        {
            G_control_mode = RC_test;
        }
        else if(G_djirc.channel.SW_L == RC_SW_UP && G_djirc.channel.SW_R == RC_SW_DOWN)
        {
            G_control_mode = KEYMOUSE;
        }
        else
        {
            G_control_mode = SAFE;
        }
    }
//TODO: 【云台】设置云台模式：GIMBAL_AUTO, GIMBAL_MANNAL, GIMBAL_SAFE
    // Set sentry robot gimbal mode
    if (G_control_mode == AUTO_G_HALFAUTO_S || G_control_mode == AUTO_CGS)
    {
        G_sentry.SetGimbalMode(SentryRobot::GIMBAL_AUTO);
    }
    else if (G_control_mode == RC_GS || G_control_mode == RC_GC)
    {
        G_sentry.SetGimbalMode(SentryRobot::GIMBAL_MANNAL);
    }
    else if (G_control_mode == RC_test)
    {
        G_sentry.SetGimbalMode(SentryRobot::GIMBAL_SAFE);
    }
    else if (G_control_mode == KEYMOUSE)
    {
         G_sentry.SetGimbalMode(SentryRobot::GIMBAL_KEYMOUSE);
    }
    else
    {
        G_sentry.SetGimbalMode(SentryRobot::GIMBAL_SAFE);
    }
//TODO: 【发射】设置射击模式
    // Set sentry robot shoot mode
    if ((G_control_mode == AUTO_CGS))
    {
        G_sentry.SetShootMode(SentryRobot::SHOOT_AUTO);
    }
    else if (G_control_mode == RC_GS)
    {
        G_sentry.SetShootMode(SentryRobot::SHOOT_MANNAL);
    }
    else if (G_control_mode == AUTO_G_HALFAUTO_S)
    {
        G_sentry.SetShootMode(SentryRobot::SHOOT_HALFAUTO);
    }
    else if (G_control_mode == KEYMOUSE)
    {
        G_sentry.SetShootMode(SentryRobot::SHOOT_KEYMOUSE);
    }
    else
    {
        G_sentry.SetShootMode(SentryRobot::SHOOT_SAFE);
    }
//TODO: 【底盘】设置底盘模式
    // Set sentry robot chassis mode
    if (G_control_mode == AUTO_CGS)
    {
        G_sentry.SetChassisMode(SentryRobot::CHASSIS_AUTO);
    }
    else if (G_control_mode == RC_GC)
    {
        G_sentry.SetChassisMode(SentryRobot::CHASSIS_MANNAL);
    }
    else if (G_control_mode == RC_test)
    {
        G_sentry.SetChassisMode(SentryRobot::CHASSIS_TEST);
    }
    else if (G_control_mode == KEYMOUSE)
    {
        G_sentry.SetChassisMode(SentryRobot::CHASSIS_KEYMOUSE);
    }
    else
    {
        G_sentry.SetChassisMode(SentryRobot::CHASSIS_SAFE);
    }

//TODO: 【底盘】设置底盘工作模式 在不同控制模式下（CHASSIS_AUTO/RC_GC/AUTO_CGS）,调整角度模式（SpeedTrackFron/SpeedTrackDoub/Gyro）
    // chassis angle mode
    if(G_sentry.chassis_mode == SentryRobot::CHASSIS_AUTO)
    {
        if(G_navigation.m_data_receive_frame.m_data_c[1] == 0)
            G_sentry.chassis_w_mode = SentryRobot::SpeedTrackFron;
        else if(G_navigation.m_data_receive_frame.m_data_c[1] == 1)
            G_sentry.chassis_w_mode = SentryRobot::SpeedTrackDoub;
        else if(G_navigation.m_data_receive_frame.m_data_c[1] == 2||G_navigation.m_data_receive_frame.m_data_c[1] == 3)
            G_sentry.chassis_w_mode = SentryRobot::Gyro;
        

        if(G_system_monitor.Navigation_Receive_fps < 50)
            G_sentry.chassis_w_mode = SentryRobot::Gyro;//掉线时也保持自旋

    }
    //TODO: 【底盘】在底盘遥控时，初始上电是SpeedTrackFron模式.
    //向下转旋钮可以实现底盘模式切换，顺序为：SpeedTrackFron->SpeedTrackDoub->Tiger->Gyro->SpeedTrackFron
    //向上转旋钮可以实现云台模式切换，顺序为：云台锁定->云台跟随速度->云台锁定
    else if(G_control_mode == RC_GC)
    {
        static uint8_t change_mode_cnt1, change_mode_cnt2;
        //根据旋钮旋转到一定角度的时间切换底盘运动模式
        if(-G_djirc.m_rocker_value.knob>0.5)
        {
            if(change_mode_cnt1<220)
                change_mode_cnt1 ++;
        }
        else
            change_mode_cnt1 = 0;
        
        if(change_mode_cnt1 == 200)
        {   
            if(G_sentry.chassis_w_mode == SentryRobot::Follow)  //下方是强制转换，将整数1强制转换为SentryRobot::ChassisAngleMode枚举类型
                G_sentry.chassis_w_mode = (SentryRobot::ChassisAngleMode)1;//TODO:【修改】测试功率下的模式修改，考虑到之后可能得底盘形式，暂且保留其他模式
            else if(G_sentry.chassis_w_mode == SentryRobot::KEYMOUSE_FOLLOW || G_sentry.chassis_w_mode == SentryRobot::KEYMOUSE_SAFE)
                G_sentry.chassis_w_mode = (SentryRobot::ChassisAngleMode)0;
            else 
                G_sentry.chassis_w_mode = (SentryRobot::ChassisAngleMode)((int)G_sentry.chassis_w_mode+1);
        }
        
        if(G_djirc.m_rocker_value.knob>0.5)
        {
            if(change_mode_cnt2<220)
                change_mode_cnt2 ++;
        }
        else
            change_mode_cnt2 = 0;
        
        if(change_mode_cnt2 == 200)
        {   
            G_sentry.yaw_control_flag = !G_sentry.yaw_control_flag;
        }
    }

    else if(G_control_mode == KEYMOUSE)
    {
            //X键操作
            if(G_djirc.PRESSED_X && !PRESSED_X_pre)
            {
             pressed_time = 800;
                //G_sentry.SetChassisMode(SentryRobot::CHASSIS_SAFE);
            }
   
            pressed_time--;

            if(G_djirc.PRESSED_C && !PRESSED_C_pre)
            {
                G_sentry.gyro_control_flag++;
            }

            if(pressed_time > 10 )
            {
                G_sentry.chassis_w_mode = SentryRobot::KEYMOUSE_SAFE;
            }
            else if(G_sentry.gyro_control_flag == 1)
            {
                G_sentry.chassis_w_mode = SentryRobot::Gyro;
            }
            else
            {
                G_sentry.chassis_w_mode = SentryRobot::KEYMOUSE_FOLLOW;
            }

            if(pressed_time < -120)
            pressed_time = 0;

            if(G_sentry.gyro_control_flag == 2)
                G_sentry.gyro_control_flag = 0;
    }

    if(G_control_mode == AUTO_CGS)
        G_sentry.patrol_flag = 1;
    else if(G_sentry.gimbal_mode == SentryRobot::GIMBAL_AUTO)
    {
        static uint8_t change_mode_cnt3;
        
        if(G_djirc.m_rocker_value.knob>0.5)
        {
            if(change_mode_cnt3<220)
                change_mode_cnt3 ++;
        }
        else
            change_mode_cnt3 = 0;
        
        if(change_mode_cnt3 == 200)
            G_sentry.patrol_flag = !G_sentry.patrol_flag;
    }
    PRESSED_X_pre = G_djirc.PRESSED_X;
    PRESSED_C_pre = G_djirc.PRESSED_C;
}

//TODO: 【系统】更新系统状态
/**
 *@brief update the state of the sentry robot
 *
 *@param
 */
void RobotStatesUpdate(void)
{
    //monitor
    G_system_monitor.Gimbal_Chassis_Receive_fps = G_gimbal.m_data_receive_frame.m_sdata[0];//云控检测到的底盘接收帧率
    G_system_monitor.Gimbal_Yaw_Receive_fps     = G_gimbal.m_data_receive_frame.m_sdata[1];//之前小云控检测到的从大云控接收帧率
    G_system_monitor.Gimbal_CAN1_fps            = G_gimbal.m_data_receive_frame.m_sdata[2];//云控CAN1帧率
    G_system_monitor.Gimbal_CAN2_fps            = G_gimbal.m_data_receive_frame.m_sdata[3];//云控CAN2帧率

    // Update DJI RC rocker value 
    G_djirc.ChannelValueUpdate();

    // Update sentry robot current gimbal states
    float gimbal_pitch_angle = G_gimbal.m_data_receive_frame.m_fdata[0]; //云控imu结合小YAWh和Pitch解算而成的pitch_syn
    float gimbal_pitch_speed = G_gimbal.m_data_receive_frame.m_fdata[1];//这个应该是之前小云控发过来的imu速度
    float gimbal_yaw_angle = G_gimbal.m_data_receive_frame.m_fdata[2];//云控imu结合小YAW和Pitch解算而成的yaw_syn
    float gimbal_yaw_speed = G_gimbal.m_data_receive_frame.m_fdata[3];//这个应该是之前小云控发过来的imu速度
    float yaw_angle = G_gimbal.m_data_receive_frame.m_fdata[4];//大YAW的imu角度
    float yaw_speed = G_gimbal.m_data_receive_frame.m_fdata[5];//大YAW的imu速度
    float gimbal_pitch_encoder = G_gimbal.m_data_receive_frame.m_fdata[6];//小云控的pitch电机通过编码器得到的角度
    float gimbal_yaw_encoder = G_gimbal.m_data_receive_frame.m_fdata[7];//小yaw电机通过编码器得到的角度
    static float gimbal_yaw_angle_pre = gimbal_yaw_angle;
    static float gimbal_yaw_angle_sum = gimbal_yaw_angle;
    static float yaw_angle_pre = yaw_angle;
    static float yaw_angle_sum = yaw_angle;
    float gimbal_yaw_angle_step = gimbal_yaw_angle - gimbal_yaw_angle_pre;
    float yaw_angle_step = yaw_angle - yaw_angle_pre;

    G_sentry.gimbal_roll = G_gimbal.m_data_receive_frame.m_fdata[12];//云控imu结合小YAW和Pitch解算而成的roll_syn
//pclassify是对浮点数的分类，这种方法在处理浮点数运算时非常有用，特别是在需要确保浮点数的有效性和类型安全的场景中。
    if((fpclassify(gimbal_pitch_angle) == FP_NORMAL || fpclassify(gimbal_pitch_angle) == FP_ZERO) && (fpclassify(gimbal_pitch_speed) == FP_NORMAL || fpclassify(gimbal_pitch_speed) == FP_ZERO))
    {
        //pitch_filter
        static float gimbal_pitch_speed_filter_high;
        static float gimbal_pitch_speed_filter_low;
        static float gimbal_pitch_angle_filter;
        gimbal_pitch_speed_filter_high = LPF(gimbal_pitch_speed_filter_high, gimbal_pitch_speed, 1000, 10);//对之前小云imu数据进行滤波，现在没用了，不过倒可以对现在传来的解算得到的pitch进行滤波
        if(fabs(gimbal_pitch_speed_filter_high) > 5)//速度很快，如果速度较快，使用较大的滤波参数
            gimbal_pitch_angle_filter = LPF(gimbal_pitch_angle_filter, gimbal_pitch_angle, 1000, 200);
        else//速度很慢
            gimbal_pitch_angle_filter = LPF(gimbal_pitch_angle_filter, gimbal_pitch_angle, 1000, 20);
        
        gimbal_pitch_speed_filter_low = LPF(gimbal_pitch_speed_filter_low, gimbal_pitch_speed, 1000, 400);

        G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->AngleUpdate(2,gimbal_pitch_angle_filter);
        G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->SpeedUpdate(2,gimbal_pitch_speed); //速度没有滤波   
    }
    
    if((fpclassify(gimbal_yaw_angle) == FP_NORMAL || fpclassify(gimbal_yaw_angle) == FP_ZERO))
    {
        if(gimbal_yaw_angle_step > 180.0f)
            gimbal_yaw_angle_step -= 360.0f;
        else if(gimbal_yaw_angle_step < -180.0f)
            gimbal_yaw_angle_step += 360.0f;
        
        gimbal_yaw_angle_sum += gimbal_yaw_angle_step;
        gimbal_yaw_angle_pre = gimbal_yaw_angle;

        G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->AngleUpdate(2,gimbal_yaw_angle_sum);
    }
    if(fpclassify(gimbal_yaw_speed) == FP_NORMAL || fpclassify(gimbal_yaw_speed) == FP_ZERO)
        G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->SpeedUpdate(2,gimbal_yaw_speed);
    
    if(fpclassify(yaw_angle) == FP_NORMAL || fpclassify(yaw_angle) == FP_ZERO)
    {
        if(yaw_angle_step > 180.0f)
            yaw_angle_step -= 360.0f;
        else if(yaw_angle_step < -180.0f)
            yaw_angle_step += 360.0f;
        
        yaw_angle_sum += yaw_angle_step;
        yaw_angle_pre = yaw_angle;

        G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->AngleUpdate(2,yaw_angle_sum);
    }
    if(fpclassify(yaw_speed) == FP_NORMAL || fpclassify(yaw_speed) == FP_ZERO)
        G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->SpeedUpdate(2,yaw_speed);

    if(fpclassify(gimbal_pitch_encoder) == FP_NORMAL || fpclassify(gimbal_pitch_encoder) == FP_ZERO)
    {
        G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current_encoder = gimbal_pitch_encoder;
        float gimbal_pitch_min = GIMBAL_PITCH_MIN + G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current - gimbal_pitch_encoder;//TODO：que:这些encoder云控读取的电机码盘值，不过为什么要这么操作呢
        float gimbal_pitch_max = GIMBAL_PITCH_MAX + G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current - gimbal_pitch_encoder;
        
        // 桥洞
        if(G_navigation.m_data_receive_frame.m_data_c[2])
            gimbal_pitch_max = 0 + G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current - gimbal_pitch_encoder;
        
        if(fabs(G_sentry.gimbal_pitch_min-gimbal_pitch_min) > 1)
            G_sentry.gimbal_pitch_min = gimbal_pitch_min;
        if(fabs(G_sentry.gimbal_pitch_max-gimbal_pitch_max) > 1)
            G_sentry.gimbal_pitch_max = gimbal_pitch_max; //过隧道时枪管朝下
    }
    if(fpclassify(gimbal_yaw_encoder) == FP_NORMAL || fpclassify(gimbal_yaw_encoder) == FP_ZERO)
    {
        G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current_encoder = gimbal_yaw_encoder;
        float gimbal_yaw_min = GIMBAL_YAW_MIN + G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current - gimbal_yaw_encoder;//消掉电机编码器与imu解算结果误差，确保min和max是实际的最小/大角度
        float gimbal_yaw_max = GIMBAL_YAW_MAX + G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current - gimbal_yaw_encoder;
        if(fabs(G_sentry.gimbal_yaw_min-gimbal_yaw_min) > 1)
            G_sentry.gimbal_yaw_min = gimbal_yaw_min;
        if(fabs(G_sentry.gimbal_yaw_max-gimbal_yaw_max) > 1)
            G_sentry.gimbal_yaw_max = gimbal_yaw_max;
    }
    
    // update sentry robot current chassis states
    //电机当前值更新
    int i;
    for(i=0; i<4; i++)
    {
        G_sentry.chassis_line_motor[i]->SpeedUpdate(0,1);
    }
    for(i=0; i<2; i++)
    {
        G_sentry.chassis_steer_motor[i]->SpeedUpdate(0,1);//TODO：que: 保持当前速度？把_encoder的值当成current的值
        G_sentry.chassis_steer_motor[i]->AngleUpdate(0,1);
    }

    // calc transform angle
    //底盘角度更新
    float chassis2yaw_angle = -G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current_encoder;//相对是个负的
    // if(G_system_monitor.Navigation_Receive_fps > 50)
    //     G_sentry.m_chassis2world_angle_current = G_sentry.m_robot_radar_world_yaw;
    // else 底盘角度等于云台角度加上相对角度。//TODO:que: ，这里的云台角度是电机imu角度。各个角度坐标系的定义与规范？
            G_sentry.m_chassis2world_angle_current = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current + chassis2yaw_angle;//与pitch的零位计算同理，只不过分了两步
    //速度反解算
    //底盘坐标系下，向前为x正向，向左为y正向。逆时针为旋转、角度正向。
    float chassis_speed_x, chassis_speed_y, chassis_speed_w;
    float fr_speed, fl_speed, br_speed, bl_speed;
    //单位、方向匹配、这个和轮子安装方式以及旋转方向有关
    fl_speed = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FL_MOTOR]->m_speed_current / RADIAN2DEGREE_VALUE * WHEEL_RADIUS;
    fr_speed = -G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_FR_MOTOR]->m_speed_current / RADIAN2DEGREE_VALUE * WHEEL_RADIUS;
    bl_speed = G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BL_MOTOR]->m_speed_current / RADIAN2DEGREE_VALUE * WHEEL_RADIUS;
    br_speed = -G_sentry.chassis_line_motor[SentryRobot::CHASSIS_L_BR_MOTOR]->m_speed_current / RADIAN2DEGREE_VALUE * WHEEL_RADIUS;
    //四全向轮反解算，由表达式可以由三个轮子速度求出整车速度//TODO:que: 速度反解算
    chassis_speed_x =   (fr_speed + fl_speed ) / 2.0f;

    chassis_speed_y =   (bl_speed - fl_speed) / 2.0f;

    chassis_speed_w =  (fr_speed -bl_speed) / 2.0f;

    // G_vofa.m_data_send_frame.m_data[0] = chassis_speed_x;
    // G_vofa.m_data_send_frame.m_data[1] = chassis_speed_y;
    //角速度修正
    G_sentry.m_chassis_spin_speed = chassis_speed_w;//车的实际转速
    //全向轮修正
    // Update sentry robot speed
    
    G_sentry.m_chassis_speed = sqrt(chassis_speed_x*chassis_speed_x + chassis_speed_y*chassis_speed_y);
    if(G_sentry.m_chassis_speed > 0.2)
        G_sentry.m_chassis_speed2chassis_angle = safeAtan2(chassis_speed_y, chassis_speed_x) * RADIAN2DEGREE_VALUE;

    G_sentry.m_chassis_speed2world_angle = G_sentry.m_chassis_speed2chassis_angle + G_sentry.m_chassis2world_angle_current;
    G_sentry.m_chassis_speed2world_angle = RangeFmod(G_sentry.m_chassis_speed2world_angle,-180.0,180.0);

//TODO:【发射】获取当前摩擦轮速度、拨弹轮角度和速度，信息来自云控
    // Update sentry robot current shoot states
    float left_friction_speed = G_gimbal.m_data_receive_frame.m_fdata[8];//摩擦轮通过电机编码器得到的角度反馈值
    float right_friction_speed = G_gimbal.m_data_receive_frame.m_fdata[9];
    float supply_bullet_angle = G_gimbal.m_data_receive_frame.m_fdata[10];//拨弹电机通过电机编码器得到的角度反馈值
    float supply_bullet_speed = G_gimbal.m_data_receive_frame.m_fdata[11];

    if(fpclassify(left_friction_speed) == FP_NORMAL || fpclassify(left_friction_speed) == FP_ZERO)
        G_sentry.friction_motor[SentryRobot::UP_FRICTION_MOTOR]->m_smc->m_fpFB = left_friction_speed;
    if(fpclassify(right_friction_speed) == FP_NORMAL || fpclassify(right_friction_speed) == FP_ZERO)
        G_sentry.friction_motor[SentryRobot::DOWN_FRICTION_MOTOR]->m_smc->m_fpFB = right_friction_speed;
    if(fpclassify(supply_bullet_angle) == FP_NORMAL || fpclassify(supply_bullet_angle) == FP_ZERO)
        G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->AngleUpdate(2,supply_bullet_angle);
    if(fpclassify(supply_bullet_speed) == FP_NORMAL || fpclassify(supply_bullet_speed) == FP_ZERO)
        G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->SpeedUpdate(2,supply_bullet_speed);
    
    //TODO:【发射】统计发射数量
    //统计弹量
    if(!G_sentry.shoot_insurance)//射击保险
    {
        //摩擦轮转速差值
        float speed_error1 = fabs(G_sentry.friction_motor[SentryRobot::UP_FRICTION_MOTOR]->m_smc->m_fpFB) - G_sentry.demo_shoot_spe;
        float speed_error2 = fabs(G_sentry.friction_motor[SentryRobot::DOWN_FRICTION_MOTOR]->m_smc->m_fpFB) - G_sentry.demo_shoot_spe;

        //检测到打弹如果两个轮子速度都减小超过100，则认为打弹
        if(!G_sentry.friction_speed_down_flag && fabs(speed_error1) > 100 && fabs(speed_error2) > 100)
        {
            G_sentry.friction_speed_down_flag = 1;
            G_sentry.shoot_count ++;
            G_sentry.shoot_ff_flag = 1;//打弹小YAW电流补偿标志位
            G_sentry.action_time = G_system_monitor.SysTickTime - G_sentry.m_shoot_bullet_last_increase_time;
        }
        //速度恢复
        else if(G_sentry.friction_speed_down_flag && fabs(speed_error1) < 100 && fabs(speed_error2) < 100)
            G_sentry.friction_speed_down_flag = 0;
    }
    else
        G_sentry.friction_speed_down_flag = 1;//感觉是拨弹条件
    
    //TODO：【雷达】更新雷达角度 que:雷达角度是指什么？世界坐标系和各个坐标系的定义？
    static float radar_world_yaw_pre = 0;
    G_sentry.lidar_error = -90.0f;
    if (G_system_monitor.UART5_rx_fps > 20) //帧率正常
    {
        static double radar_chassis_angle_diff_filter;

        // Update sentry robot radar world_yaw
        G_sentry.m_robot_radar_world_yaw_abs = G_navigation.m_data_receive_frame.m_data_f[1] * RADIAN2DEGREE_VALUE + G_sentry.lidar_error;
        G_sentry.m_robot_radar_world_yaw_abs = RangeFmod(G_sentry.m_robot_radar_world_yaw_abs,-180, 180);

        G_sentry.m_robot_radar_world_yaw_cor_abs = G_sentry.m_robot_radar_world_yaw_abs + G_sentry.GetRadarAngleSpinDevCorrect();
        G_sentry.m_robot_radar_world_yaw_cor_abs = RangeFmod(G_sentry.m_robot_radar_world_yaw_cor_abs,-180,180);
        
        G_sentry.m_robot_radar_world_yaw = RangeFmod(G_sentry.m_robot_radar_world_yaw_cor_abs-G_sentry.m_robot_radar_world_yaw, -180, 180) + G_sentry.m_robot_radar_world_yaw;

        // 滤波，预测雷达角度，防止延时
        radar_chassis_angle_diff_filter = LPF(0, G_sentry.m_robot_radar_world_yaw-G_sentry.m_chassis2world_angle_current-radar_chassis_angle_diff_filter, 1000, 0.3)+radar_chassis_angle_diff_filter;
        // radar_chassis_angle_diff_filter = LPF(radar_chassis_angle_diff_filter, G_sentry.m_robot_radar_world_yaw-G_sentry.m_chassis2world_angle_current, 1000, 0.3);
        G_sentry.m_robot_radar_world_yaw_filter = G_sentry.m_chassis2world_angle_current + radar_chassis_angle_diff_filter;
        
        // G_sentry.test_box2 = radar_chassis_angle_diff_filter;
        // G_sentry.test_box3 = G_sentry.m_robot_radar_world_yaw-G_sentry.m_chassis2world_angle_current;
    }
    else//接收帧率太低
    {
       for(uint8_t i = 0; i < NAVIGATION_DATA_RECEIVE_FLOAT_NUM; i++)
       {
            G_navigation.m_data_receive_frame.m_data_f[i] = 0;
       }
       G_navigation.m_data_receive_frame.m_data_c[1] = 0;
    }

    if(G_system_monitor.UART4_rx_fps > 900)
    {
        G_sentry.yaw_safe_flag = G_gimbal.m_data_receive_frame.m_cdata[0];//这个是之前小云台串口4从大云台的接收帧率检测标志位，当接收帧率大于900时为1
    }
    else//接收数据帧率小于900时YAW电机不输出
    {
        G_sentry.yaw_safe_flag = 0;
    }

    //TODO:【裁判】更新裁判系统信息 功率状态更新、电容状态更新、热量更新
    // Update referee robot information
    if (G_system_monitor.UART2_rx_fps > 25)
    {
       // cap update
        float power_now = G_referee.PowerHeatData.chassis_power;
        float power_limit = G_referee.GameRobotStatus.chassis_power_limit;

        float power_buffer_now = G_referee.PowerHeatData.chassis_power_buffer;


        G_sentry.capacitor->UpdateChargingPower(power_now, power_buffer_now, power_limit);
        G_sentry.capacitor->UpdateOutState();

        // G_sentry.capacitor->SetPowDes(200);

       //heat controlTODO: que:为什么是limit-40和limit/2
    //    if((G_referee.PowerHeatData.shooter_id1_17mm_cooling_heat > G_referee.GameRobotStatus.shooter_barrel_heat_limit - 40 && G_sentry.shoot_mode == SentryRobot::SHOOT_AUTO)
    //          ||(G_referee.PowerHeatData.shooter_id1_17mm_cooling_heat > G_referee.GameRobotStatus.shooter_barrel_heat_limit - 40 && G_sentry.shoot_mode == SentryRobot::SHOOT_KEYMOUSE))
       if(G_referee.PowerHeatData.shooter_id1_17mm_cooling_heat > G_referee.GameRobotStatus.shooter_barrel_heat_limit - 40)
       {
               G_sentry.shoot_cool_flag = true;
       }
       if((G_referee.PowerHeatData.shooter_id1_17mm_cooling_heat < G_referee.GameRobotStatus.shooter_barrel_heat_limit/2))
       {
               G_sentry.shoot_cool_flag = false;   
       }
       //cool value update
       G_sentry.shoot_cool_value = G_referee.GameRobotStatus.shooter_barrel_cooling_value;

        // shoot speed filter update
        if(G_referee.ShootData.bullet_speed != G_sentry.m_shoot_speed_pre && G_referee.ShootData.bullet_speed != 0){
                G_sentry.m_shoot_speed_filter->Update(G_referee.ShootData.bullet_speed);
        }
        G_sentry.m_shoot_speed_pre = G_referee.ShootData.bullet_speed;
    }
    else//掉线了
    {
        //heat control
        G_sentry.shoot_cool_flag = false; 
        //cool value update
        G_sentry.shoot_cool_value = 8;
        // shoot speed filter update
        G_sentry.m_shoot_speed_filter->Update(28.0f);
        // cap update
        float power_limit = 50.0f;

        G_sentry.capacitor->m_power_charging = power_limit;
        G_sentry.capacitor->UpdateOutState();
        G_sentry.capacitor->SetPowDes(power_limit);
        
    }
//TODO：【决策】辅瞄更新 que：辅瞄的信息是哪里发过来的？
    // aim assist update
    G_sentry.enemy_find_flag = false;
    if (G_system_monitor.Aimassist_Receive_fps > 35) 
    {
       // enemy detect state
        if (G_aim_assist.m_data_receive_frame.m_id == 0) {                                     
            G_sentry.enemy_find_flag = false;
        } else {
            G_sentry.enemy_find_flag = true;
        }
    }

}


//TODO:【系统】更新机器人底盘、云台、发射目标
/**
 *@brief update the robot targets
 *
 *@param
 */
void RobotTargetsUpdate(void)
{
    ChassisTargetsUpdate();
    GimbalTargetsUpdate();
    ShootTargetsUpdate();
}


//TODO:【底盘】更新底盘目标
/**
 *@brief update the robot chassis targets
 *
 *@param void
 *
 * @retval void
 * 
 * @note 
 */
    void ChassisTargetsUpdate(void)
{
    //键鼠模式变量
    float fpVx_pow3 =0; //x轴速度的立方,正向现为左
    float fpVy_pow3 =0; //y轴速度的立方，正向为前
    float fpW_pow3 =0;  //角速度的立方，正向为逆时针
    static bool PRESSED_C_pre = false;//上一帧是否按下C键

    //目标设置  
    //底盘坐标系，x向前 y向左 w逆时针为正
    float speed_x_des, speed_y_des, speed_w_des;
    float yaw_direction_speed, yaw_vertical_speed;//相对于YAW速度
    float gimbal_yaw_direction_speed, gimbal_yaw_vertical_speed;//相对于小YAW速度
    if(G_sentry.chassis_mode == SentryRobot::CHASSIS_TEST)
    {
        speed_x_des = G_djirc.m_rocker_value.left_level * CHASSIS_LINE_SPEED_MAX;
        speed_y_des = -G_djirc.m_rocker_value.left_upright * CHASSIS_LINE_SPEED_MAX;
        speed_w_des = -G_djirc.m_rocker_value.right_level * CHASSIS_LINE_SPEED_MAX;

    }
    
    //手动
    else if(G_sentry.chassis_mode == SentryRobot::CHASSIS_MANNAL)
    {
      
        yaw_direction_speed = -G_djirc.m_rocker_value.left_upright * CHASSIS_LINE_SPEED_MAX;// 相对于云台向前为正方向
        if(!G_sentry.yaw_control_flag)
        {
            yaw_vertical_speed = -G_djirc.m_rocker_value.left_level * CHASSIS_LINE_SPEED_MAX;//相对于云台向左为正方向
        }
        else
        {
            yaw_vertical_speed = 0;//云台锁定（需要验证）
        }
            
        if(G_sentry.chassis_w_mode == SentryRobot::Follow)
        {
            yaw_direction_speed = -G_djirc.m_rocker_value.left_upright * CHASSIS_LINE_SPEED_MAX;
            yaw_vertical_speed = -G_djirc.m_rocker_value.left_level * CHASSIS_LINE_SPEED_MAX;
        }

        float yaw2chassis_angle = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current_encoder/RADIAN2DEGREE_VALUE;//因为电机下端与底盘相连，上端与云台相连，所以这个电机的角度就是云台相对于底盘的角度
        speed_x_des = yaw_direction_speed*cosf(yaw2chassis_angle)-yaw_vertical_speed*sinf(yaw2chassis_angle);//底盘坐标系的x轴速度
        speed_y_des = yaw_direction_speed*sinf(yaw2chassis_angle)+yaw_vertical_speed*cosf(yaw2chassis_angle);//底盘坐标系的y轴速度
        G_sentry.yaw_direction_speed = yaw_direction_speed;//给vofa的
        G_sentry.yaw_vertical_speed = yaw_vertical_speed;
        //角速度 
        float yaw_angle = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current; //que:m_angle_current和m_angle_current_encoder之间的区别？
        float gimbal_yaw_angle =G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current;
        float speed_des_angle = safeAtan2(yaw_vertical_speed,yaw_direction_speed)*RADIAN2DEGREE_VALUE + yaw_angle; //云台锁定，底盘角度跟随速度
        G_sentry.number_test1=speed_des_angle;

        G_sentry.SetChassisAngleTarget(speed_des_angle);//速度跟随，底盘角度跟随速度

        //if(G_sentry.chassis_angle_test_flag)
        //    G_sentry.m_chassis2world_angle_target += G_sentry.chassis_angle_test_target;//没有搜索到其他赋值，应该没用
        // 角度控制、角速度重置
        if(G_sentry.chassis_w_mode != SentryRobot::Gyro)
        {
            float error = G_sentry.m_chassis2world_angle_target-G_sentry.m_chassis2world_angle_current;
            static uint8_t speedtrack_flag = 0;
            
            speedtrack_flag = SimpleHysteresis(speedtrack_flag, fabs(error), 2, 4);//滞回比较
            if(speedtrack_flag)            
                G_sentry.m_chassis_angle_pid->CalWeakenPID(error);
            else
                G_sentry.m_chassis_angle_pid->CalWeakenPID(0);
            speed_w_des = -G_sentry.m_chassis_angle_pid->m_output;//             que:为什么以PID输出作为旋转角速度目标值？因为控制的就是角度
        }

    
        else
        {
            speed_w_des = -CHASSIS_SPIN_SPEED_MAX*G_djirc.m_rocker_value.right_level;
        }

    

    }
     //TODO:键鼠
     else if(G_sentry.chassis_mode == SentryRobot::CHASSIS_KEYMOUSE)
     {
            //底盘指向模式
            //Chassis_KeyMouse_Point();
            //CHASSIS_MANNAL_LINE_SPEED_MAX
            //
            if( ( G_djirc.PRESSED_W || G_djirc.PRESSED_S ) && ( G_djirc.PRESSED_A || G_djirc.PRESSED_D ) )
            {
                  G_sentry.DirectX_Coe = 0.7f;
                  G_sentry.DirectY_Coe = 0.7f;
            }
              else
            {
                G_sentry.DirectX_Coe = 1;
                G_sentry.DirectY_Coe = 1;
            }
    
            /*-----------W A S D：前进 后退 左移 右移-----------*/
            if( G_djirc.PRESSED_W )                           //前进
          {
            if( fpVy_pow3 >= 0 )
            {
                G_sentry.RampSignal( &fpVy_pow3 , pow( G_sentry.y_speed_max * G_sentry.DirectY_Coe , 3 ) , LineAccelStep );
            }
            else
            {
                G_sentry.RampSignal( &fpVy_pow3 , pow( G_sentry.y_speed_max * G_sentry.DirectY_Coe  , 3 ) , LineDecelStep );
            }
          }
           else if( G_djirc.PRESSED_S )                      //后退
          {
            if( fpVy_pow3 <= 0 )
            {
                G_sentry.RampSignal( &fpVy_pow3 , pow( -G_sentry.y_speed_max * G_sentry.DirectY_Coe  , 3 ) , LineAccelStep );
            }
            else
            {
                G_sentry.RampSignal( &fpVy_pow3 , pow( -G_sentry.y_speed_max * G_sentry.DirectY_Coe  , 3 ) , LineDecelStep );
            }
            
           }
           else                                    //不前进后退
          {
            G_sentry.RampSignal(&fpVy_pow3, 0, LineDecelStep);
            //fpVy_pow2 = 0;0
          }
        
           if( fpVy_pow3 >= 0 )
          {
            yaw_direction_speed  = (int)pow( (double)( fpVy_pow3 ) , (double)0.3333 );
          }
           else
          {
            yaw_direction_speed =  -(int)pow( (double)fabs( fpVy_pow3 ) , (double)0.3333 );
          }
        
          /*----------------------------------------------------------------------------------------*/
          if( G_djirc.PRESSED_D )                           //正常右移
          {
            if( fpVx_pow3 >= 0 )
            {
                G_sentry.RampSignal(&fpVx_pow3, pow( G_sentry.x_speed_max * G_sentry.DirectX_Coe , 3 ) , LineAccelStep);
            }
            else
            {
                G_sentry.RampSignal(&fpVx_pow3, pow( G_sentry.x_speed_max * G_sentry.DirectX_Coe , 3 ) , LineDecelStep);
            }
            
           }
           else if( G_djirc.PRESSED_A )                      //正常左移
          {
            if( fpVx_pow3 <= 0 )
            {
                G_sentry.RampSignal(&fpVx_pow3, pow( -G_sentry.x_speed_max * G_sentry.DirectX_Coe , 3 ) , LineAccelStep);
            }
            else
            {
                G_sentry.RampSignal(&fpVx_pow3, pow( -G_sentry.x_speed_max * G_sentry.DirectX_Coe , 3 ) , LineDecelStep);
            }
            
          }
          else                                    //不左右移动
         {
            G_sentry.RampSignal(&fpVx_pow3, 0, LineDecelStep);    //减速
         }
    
    
    
          if(!G_sentry.yaw_control_flag)
         {
            if( fpVx_pow3 >= 0 )
             yaw_vertical_speed = pow( (double)fabs( fpVx_pow3 ) , (double)0.3333 );//相对于云台向左为正方向
            else
             yaw_vertical_speed = -pow( (double)fabs( fpVx_pow3 ) , (double)0.3333 );
         }  
         else 
         {
             if( fpVx_pow3 >= 0 )
             yaw_vertical_speed = pow( (double)fabs( fpVx_pow3 ) , (double)0.3333 );//相对于云台向左为正方向
            else
             yaw_vertical_speed = -pow( (double)fabs( fpVx_pow3 ) , (double)0.3333 );
         }
         // //TODO:或许此处并没有必要
         //  else
         //     yaw_vertical_speed = 0;
    
             float yaw2chassis_angle = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current_encoder/RADIAN2DEGREE_VALUE;//因为电机下端与底盘相连，上端与云台相连，所以这个电机的角度就是云台相对于底盘的角度
             speed_x_des = yaw_direction_speed*cosf(yaw2chassis_angle)-yaw_vertical_speed*sinf(yaw2chassis_angle);//底盘坐标系的x轴速度
             speed_y_des = yaw_direction_speed*sinf(yaw2chassis_angle)+yaw_vertical_speed*cosf(yaw2chassis_angle);//底盘坐标系的y轴速度
             G_sentry.yaw_direction_speed = yaw_direction_speed;//给vofa的
             G_sentry.yaw_vertical_speed = yaw_vertical_speed;//没用
             //角速度 
             float yaw_angle = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current; //que:m_angle_current和m_angle_current_encoder之间的区别？
             float gimbal_yaw_angle =G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current;
             float speed_des_angle = safeAtan2(yaw_vertical_speed,yaw_direction_speed)*RADIAN2DEGREE_VALUE + yaw_angle; //云台锁定，底盘角度跟随速度
             G_sentry.number_test1=speed_des_angle;
 
             G_sentry.SetChassisAngleTarget(speed_des_angle);//速度跟随，底盘角度跟随速度
    
            if(G_sentry.chassis_angle_test_flag)
                G_sentry.m_chassis2world_angle_target += G_sentry.chassis_angle_test_target;
    
              // 角度控制、角速度重置
              if(G_sentry.chassis_w_mode != SentryRobot::Gyro)
              {
                  float error = G_sentry.m_chassis2world_angle_target-G_sentry.m_chassis2world_angle_current;
                  static uint8_t speedtrack_flag = 0;
                  speedtrack_flag = SimpleHysteresis(speedtrack_flag, fabs(error), 2, 4);
                  if(speedtrack_flag)            
                      G_sentry.m_chassis_angle_pid->CalWeakenPID(error);
                  else
                      G_sentry.m_chassis_angle_pid->CalWeakenPID(0);
                  speed_w_des = -G_sentry.m_chassis_angle_pid->m_output;//             que:为什么以PID输出作为旋转角速度目标值？因为控制的就是角度
              }
              //TODO:键鼠模式下的小陀螺
              else if (G_sentry.gyro_control_flag == 1)//小陀螺
              {
                 // G_sentry.gyro_control_flag++;
                
                 G_sentry.RampSignal( &fpW_pow3 , pow(G_sentry.Top_Speed_Keyboard,3) , G_sentry.WAccelStep );
                 speed_w_des = pow( (double)fabs(fpW_pow3) , (double)0.3333 );
                
                
              }
              else 
              {
                               
                 G_sentry.RampSignal( &fpW_pow3 , 0 , G_sentry.WAccelStep );
                 speed_w_des = pow( (double)fabs(fpW_pow3) , (double)0.3333 );
              }
 
             //  if(G_sentry.gyro_control_flag == 1)
     }

    //自动
    else if(G_sentry.chassis_mode == SentryRobot::CHASSIS_AUTO)
    {
        float world_line_speed_x_rate = 0.0f;
        float world_line_speed_y_rate = 0.0f;
        
        //世界坐标系赋值
        if(G_system_monitor.Navigation_Receive_fps > 50)
        {
            if(fpclassify(G_navigation.m_data_receive_frame.m_data_f[0]) == FP_NORMAL)
                world_line_speed_x_rate = G_navigation.m_data_receive_frame.m_data_f[0];
            if(fpclassify(G_navigation.m_data_receive_frame.m_data_f[2]) == FP_NORMAL)
                world_line_speed_y_rate = G_navigation.m_data_receive_frame.m_data_f[2];
        }
        
        //坐标系转化
        float world_line_speed = sqrt(pow(world_line_speed_x_rate,2)+pow(world_line_speed_y_rate,2));
        G_sentry.m_chassis_speed2radar_angle_target =  safeAtan2(world_line_speed_y_rate,world_line_speed_x_rate)*RADIAN2DEGREE_VALUE;//全局坐标系的运动角度
        float speed2world_angle = G_sentry.m_chassis_speed2radar_angle_target - G_sentry.m_robot_radar_world_yaw_filter + G_sentry.m_chassis2world_angle_current;//底盘角度与雷达角度之差//que:为什么不直接加上滤波器的输出？
        G_sentry.m_chassis_speed2chassis_angle_target = G_sentry.m_chassis_speed2radar_angle_target - G_sentry.m_robot_radar_world_yaw_filter;

        
        G_sentry.m_chassis_speed2chassis_angle_target = RangeFmod(G_sentry.m_chassis_speed2chassis_angle_target, -180.0, 180.0);


        speed_x_des = (world_line_speed*cosf(G_sentry.m_chassis_speed2chassis_angle_target/RADIAN2DEGREE_VALUE));
        speed_y_des = (world_line_speed*sinf(G_sentry.m_chassis_speed2chassis_angle_target/RADIAN2DEGREE_VALUE));
        
        //角速度
        if(world_line_speed > 0.2)//que
            G_sentry.SetChassisAngleTarget(speed2world_angle);
        // 角度控制、角速度重置
        if(G_sentry.chassis_w_mode != SentryRobot::Gyro)
        {
            float error = G_sentry.m_chassis2world_angle_target-G_sentry.m_chassis2world_angle_current;
            static uint8_t speedtrack_flag = 0;
            speedtrack_flag = SimpleHysteresis(speedtrack_flag, fabs(error), 2, 4);
            if(speedtrack_flag)            
                G_sentry.m_chassis_angle_pid->CalWeakenPID(error);
            else
                G_sentry.m_chassis_angle_pid->CalWeakenPID(0);
            speed_w_des = -(G_sentry.m_chassis_angle_pid->m_output) ;
        }
        else if(G_system_monitor.Navigation_Receive_fps > 50)
        {
            if(G_navigation.m_data_receive_frame.m_data_c[1] == 2)
                speed_w_des = 3;
            else if(G_navigation.m_data_receive_frame.m_data_c[1] == 3)
                speed_w_des = CHASSIS_SPIN_SPEED_MAX;
            else
                speed_w_des = 0;
        }
        else
        {
            if(G_referee.GameStatus.game_progress == 4 && 
            ((G_referee.GameRobotStatus.robot_id == ROBOT_ID_SENTRY_RED&&G_referee.GameRobotHP.red_outpost_HP==0)||(G_referee.GameRobotStatus.robot_id != ROBOT_ID_SENTRY_RED&&G_referee.GameRobotHP.blue_outpost_HP==0)))
                speed_w_des = -6;
            else
                speed_w_des = 0;
        }
    }
    //安全
    else
    {
        speed_x_des = 0;
        speed_y_des = 0;
        speed_w_des = 0;
        G_sentry.m_chassis2world_angle_target = G_sentry.m_chassis2world_angle_current;
    }


    //控制优化
    if(G_sentry.chassis_mode != SentryRobot::CHASSIS_SAFE)
    {
        //底盘跟随优先
        G_sentry.ChassisSpeedtrackOptimize(1);
        //功率控制策略·改
        // G_sentry.ChassisPowControl_SpeedSet(1);
        //响应优化
        G_sentry.ChassisResponseOptimize(0);
    }
    else if(G_sentry.chassis_mode == SentryRobot::CHASSIS_TEST)
    {
        G_sentry.m_chassislimit_response_k = 1;
        G_sentry.m_chassislimit_speedtrack_k = 1;
        G_sentry.m_chassis_resposns_opti_pid->m_error_sum = 0;//未得到引用
    }
    else
    {
        G_sentry.m_chassislimit_response_k = 1;
        G_sentry.m_chassislimit_speedtrack_k = 1;
        G_sentry.m_chassis_resposns_opti_pid->m_error_sum = 0;//未得到引用
    }
    speed_x_des*=G_sentry.m_chassislimit_speedtrack_k;
    speed_y_des*=G_sentry.m_chassislimit_speedtrack_k;
    speed_w_des*=1;
    G_sentry.m_chassis_speed_target = sqrt(pow(speed_x_des,2)+pow(speed_y_des,2));
    G_sentry.m_chassis_spin_speed_target = speed_w_des;
    G_sentry.speed_x_des = speed_x_des;
    G_sentry.speed_y_des = speed_y_des;
    G_sentry.speed_w_des = speed_w_des;
//TODO:【底盘】底层解算
    //底层解算
    //轮速度目标值
    float fl_speed_x_des, fl_speed_y_des, fl_speed_des;
    float fr_speed_x_des, fr_speed_y_des, fr_speed_des;
    float bl_speed_x_des, bl_speed_y_des,  bl_speed_des;
    float br_speed_x_des, br_speed_y_des,  br_speed_des;


    fl_speed_x_des = speed_x_des - speed_w_des * (WHEEL_L2R_DIST/2) / 1000.0 ;
    fl_speed_y_des = speed_y_des + speed_w_des * (WHEEL_F2B_DIST/2) / 1000.0 ;
    fr_speed_x_des = speed_x_des + speed_w_des * (WHEEL_L2R_DIST/2) / 1000.0 ;
    fr_speed_y_des = speed_y_des + speed_w_des * (WHEEL_F2B_DIST/2) / 1000.0 ;
    bl_speed_x_des = speed_x_des - speed_w_des * (WHEEL_L2R_DIST/2) / 1000.0 ;
    bl_speed_y_des = speed_y_des - speed_w_des * (WHEEL_F2B_DIST/2) / 1000.0 ;
    br_speed_x_des = speed_x_des + speed_w_des * (WHEEL_L2R_DIST/2) / 1000.0 ;
    br_speed_y_des = speed_y_des - speed_w_des * (WHEEL_F2B_DIST/2) / 1000.0 ;

    //左前轮速度目标值，向右前为正，米每秒
    fl_speed_des = fl_speed_x_des * cosf(WHEEL_ANGLE / RADIAN2DEGREE_VALUE) - fl_speed_y_des * sinf(WHEEL_ANGLE / RADIAN2DEGREE_VALUE);

    //右前轮速度目标值，向左前为正，米每秒
    fr_speed_des = fr_speed_x_des * cosf(WHEEL_ANGLE / RADIAN2DEGREE_VALUE) + fr_speed_y_des * sinf(WHEEL_ANGLE / RADIAN2DEGREE_VALUE);

    //左后轮速度目标值，向右前为正，米每秒
    bl_speed_des = bl_speed_x_des * cosf(WHEEL_ANGLE / RADIAN2DEGREE_VALUE) + bl_speed_y_des * sinf(WHEEL_ANGLE / RADIAN2DEGREE_VALUE);

    //右后轮速度目标值，向左前为正，米每秒
    br_speed_des = br_speed_x_des * cosf(WHEEL_ANGLE / RADIAN2DEGREE_VALUE) - br_speed_y_des * sinf(WHEEL_ANGLE / RADIAN2DEGREE_VALUE);

    //单位、具体方向适配
    //实际正向：
    //FL 右前
    //FR 右后
    //BL 舵轮同向
    //BR 舵轮反向
    fl_speed_des = fl_speed_des / WHEEL_RADIUS * RADIAN2DEGREE_VALUE;//换算成 角度/s
    fl_speed_des = -fl_speed_des;
    fr_speed_des = fr_speed_des / WHEEL_RADIUS * RADIAN2DEGREE_VALUE;
    bl_speed_des = bl_speed_des / WHEEL_RADIUS * RADIAN2DEGREE_VALUE;
    bl_speed_des = -bl_speed_des;
    br_speed_des = br_speed_des / WHEEL_RADIUS * RADIAN2DEGREE_VALUE;
		G_sentry.fl_speed_des = fl_speed_des;
		G_sentry.fr_speed_des = fr_speed_des;
		G_sentry.bl_speed_des = bl_speed_des;
		G_sentry.br_speed_des = br_speed_des;
		G_sentry.speedw_des = speed_w_des;  

    PRESSED_C_pre = G_djirc.PRESSED_C;

    //目标值赋值
    G_sentry.SetChassisSpeedTarget(fl_speed_des, bl_speed_des,fr_speed_des, br_speed_des);
}



//TODO: 【云台】更新云台目标值
/**
 *@brief update the robot gimbal targets
 *
 *@param
 */
void GimbalTargetsUpdate(void)
{

    static bool PRESSED_X_Pre = false;//上一帧是否按下X键
    float gimbal_pitch_step = 0; //TODO: 【云台】?pitch增量，单位：度，正为俯仰，负为仰视？
    float gimbal_yaw_step = 0;
    float yaw_step = 0;

    G_sentry.enemy_track_flag = 0;
    
    if(G_sentry.gimbal_mode == SentryRobot::GIMBAL_MANNAL)
    {
        if(G_sentry.gimbal_test_flag)
            Gimbal_Test();
        else
        {
            //遥控器控制底盘时底盘云台跟随
            if(G_sentry.chassis_mode == SentryRobot::CHASSIS_MANNAL)
            {
                if (fabs((float)G_djirc.channel.Ch0 - RC_CH_VALUE_OFFSET) > RC_CH_VALUE_DEAD)
                {
                    if(G_sentry.yaw_control_flag)
                    {
                        
                        yaw_step = -((float)(G_djirc.channel.Ch0 - RC_CH_VALUE_OFFSET) / 
                        RC_CH_VALUE_RANGE * GIMBAL_PITCH_ANGLE_MANNAL_CONTROL_SENSITIVITY*5);
                        gimbal_yaw_step = yaw_step;//大yaw动的时候小yaw不动
                     
                    }
                }
                //if (fabs((float)G_djirc.channel.Ch0 - RC_CH_VALUE_OFFSET) > RC_CH_VALUE_DEAD)
                //{
                //    if(G_sentry.yaw_control_flag)
                //    {
                //        yaw_step = -((float)(G_djirc.channel.Ch0 - RC_CH_VALUE_OFFSET) / 
                //        RC_CH_VALUE_RANGE * GIMBAL_PITCH_ANGLE_MANNAL_CONTROL_SENSITIVITY*5);
                //        gimbal_yaw_step = yaw_step;
                //    }
                //}
                //if (fabs((float)G_djirc.channel.Ch1 - RC_CH_VALUE_OFFSET) > RC_CH_VALUE_DEAD)
                //{
                //    if(G_sentry.yaw_control_flag)
                //    {
                //       yaw_step = -((float)(G_djirc.channel.Ch1 - RC_CH_VALUE_OFFSET) / 
                //        RC_CH_VALUE_RANGE * GIMBAL_PITCH_ANGLE_MANNAL_CONTROL_SENSITIVITY*5);
                //        gimbal_pitch_step = yaw_step;
                //    }
                //}
            }
            //在遥控底盘模式下不用该摇杆操作
            if(G_control_mode != RC_GC)//单独控制云台时
            {
                if (fabs((float)G_djirc.channel.Ch0 - RC_CH_VALUE_OFFSET) > RC_CH_VALUE_DEAD)
                {
                    gimbal_yaw_step = -((float)(G_djirc.channel.Ch0 - RC_CH_VALUE_OFFSET) / 
                    RC_CH_VALUE_RANGE * GIMBAL_YAW_ANGLE_MANNAL_CONTROL_SENSITIVITY);
                }

                if (fabs((float)G_djirc.channel.Ch1 - RC_CH_VALUE_OFFSET) > RC_CH_VALUE_DEAD)
                {
                    gimbal_pitch_step = -((float)(G_djirc.channel.Ch1 - RC_CH_VALUE_OFFSET) / 
                    RC_CH_VALUE_RANGE * GIMBAL_PITCH_ANGLE_MANNAL_CONTROL_SENSITIVITY);
                }

                if (fabs((float)G_djirc.channel.Ch2 - RC_CH_VALUE_OFFSET) > RC_CH_VALUE_DEAD)
                {
                    yaw_step = -((float)(G_djirc.channel.Ch2 - RC_CH_VALUE_OFFSET) / 
                    RC_CH_VALUE_RANGE * GIMBAL_PITCH_ANGLE_MANNAL_CONTROL_SENSITIVITY);
                }
            }
            G_sentry.gimbal_pitch_des += gimbal_pitch_step;
            G_sentry.gimbal_yaw_des += gimbal_yaw_step;
            G_sentry.yaw_des += yaw_step;
        }
    }
    //TODO: 【云台键鼠】鼠标控制云台
     //TODO:【云台键鼠】现在已加云台辅瞄，但是长按鼠标右键并且满足辅瞄条件时进入辅瞄，不按右键或不满足辅瞄判断时再退出（辅瞄期间内鼠标移动无法让云台移动） 
     else if(G_sentry.gimbal_mode == SentryRobot::GIMBAL_KEYMOUSE)
     {
        static bool auto_flag = false;//辅瞄标志位
        //  //TODO: 【云台辅瞄】鼠标右键长按计时(射击键鼠模式下已加计时，此处防止重复)
        // if(G_sentry.stMouse.Right == 1)
        // {
        //     G_sentry.Shoot_KEYMOUSE.Key_Righting_Time_Cnt++;
        // }
        // else
        // {
        //     G_sentry.Shoot_KEYMOUSE.Key_Righting_Time_Cnt = 0;
        // }

        //是否进入辅瞄模式判断
        if(( G_sentry.Shoot_KEYMOUSE.Key_Righting_Time_Cnt >= 280
            && (G_aim_assist.m_data_receive_frame.m_data_f[0] > (G_sentry.gimbal_pitch_min + 1.0f) && G_aim_assist.m_data_receive_frame.m_data_f[0] < (G_sentry.gimbal_pitch_max - 1.0f)))
            || (G_sentry.Shoot_KEYMOUSE.Key_Righting_Time_Cnt >= 280
            && (G_aim_assist.m_data_receive_frame.m_data_f[0] > G_sentry.gimbal_pitch_min && G_aim_assist.m_data_receive_frame.m_data_f[0] < G_sentry.gimbal_pitch_max)))
        {
            auto_flag = true;//可以切换到辅瞄模式
        }
        else
        {
            auto_flag = false;
        }

       //云台键鼠模式下不同模式细分
         if(G_sentry.gimbal_test_flag)
             Gimbal_Test();
         //辅瞄模式//辅瞄抓到目标
         else if(G_sentry.enemy_find_flag && auto_flag)
         {
            if(fpclassify(G_aim_assist.m_data_receive_frame.m_data_f[0]) == FP_NORMAL ||fpclassify(G_aim_assist.m_data_receive_frame.m_data_f[0]) == FP_ZERO)
                G_sentry.gimbal_pitch_des = G_aim_assist.m_data_receive_frame.m_data_f[0];
            if(fpclassify(G_aim_assist.m_data_receive_frame.m_data_f[1]) == FP_NORMAL || fpclassify(G_aim_assist.m_data_receive_frame.m_data_f[1]) == FP_ZERO)
            {
                G_sentry.gimbal_yaw_des = Clip(G_aim_assist.m_data_receive_frame.m_data_f[1],G_sentry.gimbal_yaw_min,G_sentry.gimbal_yaw_max);
                
                static uint8_t yaw_track_flag = 0;
                float yaw_error;  
                yaw_error =         + G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current_encoder
                                    + G_aim_assist.m_data_receive_frame.m_data_f[1]
                                    - G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current;              
                // yaw_error =         RangeFmod(yaw_error,-180, 180);
                // yaw_track_flag =    SimpleHysteresis(yaw_track_flag, fabs(yaw_error), 10, 45);//滞回比较器
                // if(yaw_track_flag)
                    G_sentry.yaw_des = yaw_error + G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current;
            }

            //跟上辅瞄目标
            if( fabs(G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current-G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_target) < Pitch_Vision_Error &&
                fabs(G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current-G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_target) < Yaw_Vision_Error 
            )
                G_sentry.enemy_track_flag = 1;
                
            //G_sentry.m_vision_delay_cnt = 0;
         }
        //非辅瞄模式
         else 
         {       
         yaw_step = G_sentry.KeyMouse_SST_Yaw * G_sentry.stMouse.X;
         gimbal_yaw_step = G_sentry.KeyMouse_SST_Yaw   * G_sentry.stMouse.X;
         gimbal_pitch_step = G_sentry.KeyMouse_SST_Pitch * G_sentry.stMouse.Y;
  
         G_sentry.yaw_des -= yaw_step;
         G_sentry.gimbal_yaw_des   -= gimbal_yaw_step;
         G_sentry.gimbal_yaw_des = Clip(G_sentry.gimbal_yaw_des,G_sentry.gimbal_yaw_min,G_sentry.gimbal_yaw_max);
        
         G_sentry.gimbal_pitch_des -= gimbal_pitch_step;
         G_sentry.gimbal_pitch_des = Clip(G_sentry.gimbal_pitch_des, G_sentry.gimbal_pitch_min, G_sentry.gimbal_pitch_max);
         }

        if (G_djirc.PRESSED_X == true && PRESSED_X_Pre == false ) 
        {
            if (G_sentry.rotation_state ==SentryRobot:: ROTATION_IDLE) 
            {
                G_sentry.rotation_state = SentryRobot::ROTATION_IN_PROGRESS; // 启动旋转过程
                G_sentry.rotation_count = 0; // 重置计数器
            }
        }
        //TODO:一个想法：在云台旋转的过程中，或许可以让小yaw先转，大yaw后转，这样可以减少小yaw的误差
        if (G_sentry.rotation_state == SentryRobot::ROTATION_IN_PROGRESS) 
        {
            if (G_sentry.rotation_count < TOTAL_ROTATION_COUNT) 
            { // 6次旋转，每次30°，总共180°
                yaw_step = ROTATION_STEP  ;//每次旋转步长
                G_sentry.yaw_des += yaw_step;
                G_sentry.gimbal_yaw_des += yaw_step;
                G_sentry.gimbal_yaw_des = Clip(G_sentry.gimbal_yaw_des, G_sentry.gimbal_yaw_min, G_sentry.gimbal_yaw_max);
                G_sentry.gimbal_pitch_des += 0;
                G_sentry.rotation_count++; // 增加计数器
                G_sentry.last_rotation_time = G_sentry.gimbal_turn_current_time; // 记录本次旋转完成的时间
                G_sentry.rotation_state = SentryRobot::ROTATION_WAITING; // 进入等待状态
            } 
            else
            {
                G_sentry.rotation_state = SentryRobot::ROTATION_IDLE; // 旋转完成，回到空闲状态
            }
        } 
        else if (G_sentry.rotation_state == SentryRobot::ROTATION_WAITING) 
        {
            if (G_sentry.gimbal_turn_current_time - G_sentry.last_rotation_time >= G_sentry.rotation_delay) 
            {
                G_sentry.rotation_state = SentryRobot::ROTATION_IN_PROGRESS; // 等待时间结束，继续旋转
            }
        } 
        else if (G_sentry.rotation_state == SentryRobot::ROTATION_IDLE) 
        {
            yaw_step = 0;
            G_sentry.yaw_des += yaw_step;
            G_sentry.gimbal_yaw_des += yaw_step;
            G_sentry.gimbal_yaw_des = Clip(G_sentry.gimbal_yaw_des, G_sentry.gimbal_yaw_min, G_sentry.gimbal_yaw_max);
            G_sentry.gimbal_pitch_des += 0;
        }
 
     }

    else if(G_sentry.gimbal_mode == SentryRobot::GIMBAL_AUTO)
    {
        static bool auto2scan_flag = false;//用于巡逻与辅瞄切换，防止在pitch云台限位附近反复切换
        bool auto_flag = false;
        
        //防止巡逻与索敌模式反复切换
        if((!auto2scan_flag 
            && (G_aim_assist.m_data_receive_frame.m_data_f[0] > (G_sentry.gimbal_pitch_min + 1.0f) && G_aim_assist.m_data_receive_frame.m_data_f[0] < (G_sentry.gimbal_pitch_max - 1.0f)))
            || (auto2scan_flag
            && (G_aim_assist.m_data_receive_frame.m_data_f[0] > G_sentry.gimbal_pitch_min && G_aim_assist.m_data_receive_frame.m_data_f[0] < G_sentry.gimbal_pitch_max)))
        {
            auto_flag = true;//可以切换到辅瞄模式
        }

        //辅瞄抓到目标
        if(G_sentry.enemy_find_flag && auto_flag)
        {
            if(fpclassify(G_aim_assist.m_data_receive_frame.m_data_f[0]) == FP_NORMAL ||fpclassify(G_aim_assist.m_data_receive_frame.m_data_f[0]) == FP_ZERO)
                G_sentry.gimbal_pitch_des = G_aim_assist.m_data_receive_frame.m_data_f[0];
            if(fpclassify(G_aim_assist.m_data_receive_frame.m_data_f[1]) == FP_NORMAL || fpclassify(G_aim_assist.m_data_receive_frame.m_data_f[1]) == FP_ZERO)
            {
                G_sentry.gimbal_yaw_des = Clip(G_aim_assist.m_data_receive_frame.m_data_f[1],G_sentry.gimbal_yaw_min,G_sentry.gimbal_yaw_max);
                
                static uint8_t yaw_track_flag = 0;
                float yaw_error;  
                yaw_error =         + G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current_encoder
                                    + G_aim_assist.m_data_receive_frame.m_data_f[1]
                                    - G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current;              
                // yaw_error =         RangeFmod(yaw_error,-180, 180);
                yaw_track_flag =    SimpleHysteresis(yaw_track_flag, fabs(yaw_error), 10, 45);//滞回比较器
                if(yaw_track_flag)
                    G_sentry.yaw_des = yaw_error + G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current;
            }

            //跟上辅瞄目标
            if( fabs(G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current-G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_target) < Pitch_Vision_Error &&
                fabs(G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current-G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_target) < Yaw_Vision_Error 
            )
                G_sentry.enemy_track_flag = 1;
                
            G_sentry.m_vision_delay_cnt = 0;
            G_sentry.direction_flag = true;
            auto2scan_flag = true;
        }
        //巡逻
        else
        {
            //先保持当前姿态一段时间再巡逻
            if(G_sentry.m_vision_delay_cnt < 500)
            {
                gimbal_pitch_step = 0;
                gimbal_yaw_step = 0;
                yaw_step = 0;
                G_sentry.m_vision_delay_cnt++;
            }
            else
            {
                if(G_sentry.patrol_flag)
                {
                    if(G_sentry.gimbal_pitch_scan_dir)//巡逻方向判断
                        gimbal_pitch_step = GIMBAL_PITCH_ANGLE_SCAN_SPEED;
                    else
                        gimbal_pitch_step = -GIMBAL_PITCH_ANGLE_SCAN_SPEED;
                    if(G_sentry.gimbal_yaw_scan_dir)
                    {
                        gimbal_yaw_step = GIMBAL_YAW_ANGLE_SCAN_SPEED;
                    }
                    else  
                    {
                        gimbal_yaw_step = - GIMBAL_YAW_ANGLE_SCAN_SPEED;
                    }  
                        
                    if(G_sentry.yaw_scan_dir)
                        yaw_step = YAW_ANGLE_SCAN_SPEED;
                    else
                        yaw_step = - YAW_ANGLE_SCAN_SPEED;

                    G_sentry.gimbal_pitch_des += gimbal_pitch_step;
                    G_sentry.gimbal_yaw_des += gimbal_yaw_step;
                    G_sentry.yaw_des += yaw_step;

                    if(G_sentry.gimbal_pitch_des > G_sentry.gimbal_pitch_max-5)
                        G_sentry.gimbal_pitch_scan_dir = false;
                    else if(G_sentry.gimbal_pitch_des < G_sentry.gimbal_pitch_min+5)
                        G_sentry.gimbal_pitch_scan_dir = true;
                    
                    if(G_sentry.gimbal_yaw_des > G_sentry.gimbal_yaw_max)
                        G_sentry.gimbal_yaw_scan_dir = false;
                    else if(G_sentry.gimbal_yaw_des < G_sentry.gimbal_yaw_min)
                        G_sentry.gimbal_yaw_scan_dir = true;

                    if (G_aim_assist.m_data_receive_frame.m_data_f[3] < 0.5f && G_sentry.direction_flag) 
                        G_sentry.yaw_scan_dir = false;
                    else if (G_aim_assist.m_data_receive_frame.m_data_f[3] > 0.5f && G_sentry.direction_flag) 
                        G_sentry.yaw_scan_dir = true;
                }
                G_sentry.m_vision_delay_cnt = 600;
                G_sentry.direction_flag = false;
                auto2scan_flag = false;
                // G_sentry.gimbal_pitch_des = 0;
            }
        }
    }

    else
    {
       
            G_sentry.gimbal_pitch_des = G_sentry.gimbal_motor[SentryRobot::GIMBAL_PITCH_MOTOR]->m_angle_current;
            G_sentry.gimbal_yaw_des = G_sentry.gimbal_motor[SentryRobot::GIMBAL_YAW_MOTOR]->m_angle_current;
            G_sentry.yaw_des = G_sentry.gimbal_motor[SentryRobot::YAW_MOTOR]->m_angle_current;

            for(uint8_t i = 0; i < GIMBAL_MOTOR_NUM; i++)
            {
                G_sentry.gimbal_motor[i]->m_angle_target = G_sentry.gimbal_motor[i]->m_angle_current;
                G_sentry.gimbal_motor[i]->m_angle_td->m_aim = G_sentry.gimbal_motor[i]->m_angle_target;
                G_sentry.gimbal_motor[i]->m_angle_td->m_x1 = G_sentry.gimbal_motor[i]->m_angle_target;
                G_sentry.gimbal_motor[i]->m_angle_td->m_x2 = 0;
                G_sentry.gimbal_motor[i]->m_speed_pid->m_output = 0;
            }
       
    }
    PRESSED_X_Pre = G_djirc.PRESSED_X;
    G_sentry.gimbal_pitch_des = Clip(G_sentry.gimbal_pitch_des,G_sentry.gimbal_pitch_min,G_sentry.gimbal_pitch_max);
    G_sentry.gimbal_yaw_des = Clip(G_sentry.gimbal_yaw_des,G_sentry.gimbal_yaw_min,G_sentry.gimbal_yaw_max);
    G_sentry.SetGimbalAngleTarget(G_sentry.gimbal_yaw_des,G_sentry.gimbal_pitch_des,G_sentry.yaw_des);
}

/*TODO: 【云台】调试 一切要面向实际效果！
    **调pitch:首先关掉重力补偿（ffmode=0），在不同角度观察PID的输出值，将其作为重力补偿值，重力补偿表格值的修改在sentry初始化里，可以动一下pitch，看其是否丝滑；
在重力补偿调好之后可以进行电机PID的调参，调试时可以找到gimbal_test_flag，可以看看在不同情况下的性能(gimbal_test_flag)。目标是更好地阶跃响应。
    **重力前馈电流不是在上面的操作中一次确认的，需要根据调参时具体情况具体调整。重力前馈是为了使PITCH向上和向下阶跃时响应情况（时间、大致震荡）相同。重力前馈减小
可以让整个响应曲线下移，重力前馈加大可以让整个响应曲线上移，最后使得上升和下降一致。
    **摩擦力前馈可以通过让PITCH以小速度移动，观察其电流输出，作为摩擦力前馈值。摩擦力前馈值可以加快响应速度，但过大会导致响应曲线震荡加剧。
    **TD里的m_k速度跟随可以起到加快响应的作用。
    **调参时如果参数改变没有什么效果要去思考原因，是不是当前角度不在作用区间里？把参数放大5倍试一试？
    **出现异常情况一定要记得把遥控器切换成安全模式！
    **难以解决的问题：鲁棒性不强，可能只是换个位置，之前调好的参数效果也会不太好，非常不稳定！该怎么解决呢？//TODO:【待优化】控制器、SD卡、功率-速度曲线测试、四轮上接传感器、功率控制优化
    
    **调小yaw：比较好调，PID就行
    **调大yaw：比较好调，PID就行

    //大YAW和小YAW在flag3时目标角度一样的，都是G_sentry.gimbal_yaw_test_des
*/
/**
 * @brief 云台调试
 * @return * void 
 * @author 
 * @note 依据test_flag的值进入不同的调试模式，目前有三种模式 1：手动输入目标角度，一次阶跃 2：小YAW左右扫描、pitch上下扫描 3:连续小幅度（默认5度）阶跃（常用）
 */
//调试
void Gimbal_Test(void)
{
    static float gimbal_pitch_des_pre = 0;
    static float gimbal_yaw_des_pre = 0;
    static float yaw_des_pre = 0;
    float gimbal_pitch_des_step = 0;
    float gimbal_yaw_des_step = 0;
    float yaw_des_step = 0;
    static float gimbal_pitch_des_temp;
    static float gimbal_yaw_des_temp;
    static float yaw_des_temp;

    if(G_sentry.gimbal_test_flag == 1) //
    {
        gimbal_pitch_des_temp = G_sentry.gimbal_pitch_test_des;
        gimbal_yaw_des_temp = G_sentry.gimbal_yaw_test_des;
        yaw_des_temp = G_sentry.yaw_test_des;
    }
    else if(G_sentry.gimbal_test_flag == 2)
    {
        if(G_sentry.gimbal_yaw_scan_dir)
            gimbal_pitch_des_temp += AIM_ASSIST_TEST_SPEED;
        else
            gimbal_pitch_des_temp -= AIM_ASSIST_TEST_SPEED;
        if(G_sentry.gimbal_pitch_scan_dir)
            gimbal_yaw_des_temp += AIM_ASSIST_TEST_SPEED;
        else    
            gimbal_yaw_des_temp -= AIM_ASSIST_TEST_SPEED;
        
        if(gimbal_yaw_des_temp > AIM_ASSIST_TEST_PITCH_MAX)//TODO:【云台】？当yaw超过最大值时，改变PITCH扫描方向
            G_sentry.gimbal_pitch_scan_dir = 0;
        else if(gimbal_yaw_des_temp < AIM_ASSIST_TEST_PITCH_MIN)
            G_sentry.gimbal_pitch_scan_dir = 1;
        
        if(gimbal_pitch_des_temp > AIM_ASSIST_TEST_YAW_MAX)//当pitch超过最大值时，改变小YAW扫描方向
            G_sentry.gimbal_yaw_scan_dir = 0;
        else if(gimbal_pitch_des_temp < AIM_ASSIST_TEST_YAW_MIN)
            G_sentry.gimbal_yaw_scan_dir = 1;

        yaw_des_temp = gimbal_yaw_des_temp;
    }
    else if(G_sentry.gimbal_test_flag == 3)
    {
        static uint16_t step_cnt;
        
        step_cnt ++;
        if(step_cnt > 3000)
            step_cnt = 0;

        if(step_cnt < 1500)
            gimbal_pitch_des_temp = 0;
        else if(G_sentry.gimbal_pitch_test_des != 0)
            gimbal_pitch_des_temp = G_sentry.gimbal_pitch_test_des;
        //默认2
        else
            gimbal_pitch_des_temp = 2;
        
        if(step_cnt < 500 || step_cnt > 2000)
            gimbal_yaw_des_temp = 0;
        else if(G_sentry.gimbal_yaw_test_des != 0)
            gimbal_yaw_des_temp = G_sentry.gimbal_yaw_test_des;
        //默认5
        else    
            gimbal_yaw_des_temp = 5;

        if(step_cnt < 1000 || step_cnt > 2500)
            yaw_des_temp = 0;
        else if(G_sentry.gimbal_yaw_test_des != 0)
            yaw_des_temp = G_sentry.gimbal_yaw_test_des;
        //默认5
        else    
            yaw_des_temp = 5;    
    }
//最后还是算的step增量，并且把上一次角度记录下来赋值给_temp
    gimbal_pitch_des_step = gimbal_pitch_des_temp - gimbal_pitch_des_pre;
    gimbal_pitch_des_pre = gimbal_pitch_des_temp;
    gimbal_yaw_des_step = gimbal_yaw_des_temp - gimbal_yaw_des_pre;
    gimbal_yaw_des_pre = gimbal_yaw_des_temp;
    yaw_des_step = yaw_des_temp - yaw_des_pre;
    yaw_des_pre = yaw_des_temp;

    G_sentry.gimbal_pitch_des += gimbal_pitch_des_step;
    G_sentry.gimbal_yaw_des += gimbal_yaw_des_step;
    G_sentry.yaw_des += yaw_des_step;
}

//TODO:【发射】摩擦轮启动和拨弹 
/**
 *@brief update the robot gimbal targets 摩擦轮启动和拨弹 
 *
 *@param
 *
 * @note 主要是射击拨弹，先根据模式判断是否可以射击（摩擦轮能否启动），再执行手动/半自动/自动射击
 */
void ShootTargetsUpdate(void)
{
    static uint16_t delay_cnt = 0;
    bool MouseLeftPre = false; //TODO:【发射】鼠标左键上一次状态


    //frction 先判断是否可以射击，自动模式下可以射击，半自动模式下需要判断是否转动射击旋钮
    if(G_sentry.shoot_mode == SentryRobot::SHOOT_MANNAL || G_sentry.shoot_mode == SentryRobot::SHOOT_HALFAUTO)
    {
        if (G_djirc.channel.Ch4 - RC_CH_VALUE_OFFSET < -RC_CH_VALUE_DEAD)
            G_sentry.shoot_insurance = true;
        else if (G_djirc.channel.Ch4 - RC_CH_VALUE_OFFSET > RC_CH_VALUE_DEAD)
            G_sentry.shoot_insurance = false;
    }
    else if(G_sentry.shoot_mode == SentryRobot::SHOOT_AUTO)
        G_sentry.shoot_insurance = false;
    //TODO:键鼠模式打弹
    else if(G_sentry.shoot_mode == SentryRobot::SHOOT_KEYMOUSE)
    G_sentry.shoot_insurance = false;
    else
        G_sentry.shoot_insurance = true;   
    //friction target
    if(!G_sentry.shoot_insurance)
    {
        G_sentry.friction_motor[SentryRobot::UP_FRICTION_MOTOR]->m_smc->m_fpDes = G_sentry.demo_shoot_spe;
        G_sentry.friction_motor[SentryRobot::DOWN_FRICTION_MOTOR]->m_smc->m_fpDes = -G_sentry.demo_shoot_spe;
    }
    else
    {
        G_sentry.friction_motor[SentryRobot::UP_FRICTION_MOTOR]->m_smc->m_fpDes = 0;
        G_sentry.friction_motor[SentryRobot::DOWN_FRICTION_MOTOR]->m_smc->m_fpDes = 0;

        G_sentry.friction_motor[SentryRobot::UP_FRICTION_MOTOR]->m_smc->m_fpU = 0;
        G_sentry.friction_motor[SentryRobot::DOWN_FRICTION_MOTOR]->m_smc->m_fpU = 0;
    }


    //G_sentry.Is_Heat_Safe();

    //supply_bullet    
    G_sentry.supply_bullut_reset_flag = false;
    G_sentry.m_shoot_bullet_fps = 0;
    G_sentry.enemy_attackable_flag = 0;
    //手动
    if(G_sentry.shoot_mode == SentryRobot::SHOOT_MANNAL && !G_sentry.shoot_insurance)
    {
        if (G_djirc.channel.Ch3 - RC_CH_VALUE_OFFSET < -RC_CH_VALUE_DEAD)
            G_sentry.m_shoot_bullet_fps = SHOOT_BULLET_FREQUENCY_TEST_1;
        else if(G_djirc.channel.Ch3 - RC_CH_VALUE_OFFSET > RC_CH_VALUE_DEAD && !G_sentry.shoot_cool_flag)
                G_sentry.m_shoot_bullet_fps = SHOOT_BULLET_FREQUENCY_TEST_4;
    }
    //TODO:键鼠模式(已加辅瞄)
    else if(G_sentry.shoot_mode == SentryRobot::SHOOT_KEYMOUSE && !G_sentry.shoot_insurance)
     {
    //     //TODO:【发射】鼠标右键长按的判断
        if(G_sentry.stMouse.Right == 1)
        {
            G_sentry.Shoot_KEYMOUSE.Key_Righting_Time_Cnt++;
        }
        else
        {
            G_sentry.Shoot_KEYMOUSE.Key_Righting_Time_Cnt = 0;
        }

        // //TODO:【发射先判定是否辅瞄】鼠标右键长按进入辅瞄
        if(G_sentry.Shoot_KEYMOUSE.Key_Righting_Time_Cnt >= 280)
        {
            G_sentry.Shoot_KEYMOUSE.Key_Is_Assit = 1;
        }
        else
        {
            G_sentry.Shoot_KEYMOUSE.Key_Is_Assit = 0;
        }

        //模式判断
        if(G_sentry.Shoot_KEYMOUSE.Key_Lefting_Time_Cnt >= 280)
        {
            G_sentry.Shoot_Turn_Mode = SentryRobot::RunningShoot ;
        }
        else
        {
            G_sentry.Shoot_Turn_Mode = SentryRobot::SingleShoot ;
        }

        //射击计时
        if(( G_sentry.stMouse.Left == 1 ))//&& system_monitor.CAN_Rx_LeftFritionWheel_fps>=700 && G_ST_Bullet_Remaining.projectile_allowance_17mm > 4  ))                                   //对鼠标单击一下的最小时间间隔的判断120ms
        {
            if( G_sentry.Shoot_KEYMOUSE.Key_Left_Time_Cnt >= 0 
                    || ( G_sentry.Shoot_Turn_Mode == SentryRobot::RunningShoot ) 
              )
            {
                G_sentry.Shoot_KEYMOUSE.Key_Left_Time_Cnt = -120 ;//每按一下就重新计算单击鼠标的周期
                G_sentry.Shoot_KEYMOUSE.Key_Is_Shoot = 1 ;                                  
            }
            G_sentry.Shoot_KEYMOUSE.Key_Lefting_Time_Cnt++; //对长按鼠标计时
        }
        else
        {
            G_sentry.Shoot_KEYMOUSE.Key_Lefting_Time_Cnt = 0 ;                      //一旦松开鼠标，长按计时就清零
            G_sentry.Shoot_KEYMOUSE.Key_Is_Shoot = 0 ;
            G_sentry.Shoot_KEYMOUSE.Key_Left_Time_Cnt++;
        }

      //射击频率设定
    //   //TODO:键鼠下的辅瞄模式（在辅瞄模式下加入视觉判断）【但还是会由左键判断单发或者连发】 
      if(G_sentry.Shoot_KEYMOUSE.Key_Is_Assit == 1  &&  G_sentry.Is_Heat_Safe() == true )
      {                                                                                         
        if(G_sentry.Shoot_Turn_Mode == SentryRobot::SingleShoot && G_sentry.Shoot_KEYMOUSE.Key_Is_Shoot == 1 && G_aim_assist.m_data_receive_frame.m_data_f[2] > 0.5 && G_sentry.enemy_track_flag && G_sentry.Is_Heat_Safe() == true )// && !G_sentry.shoot_cool_flag
        {
            G_sentry.m_shoot_bullet_fps = SHOOT_BULLET_FREQUENCY_TEST_1;
        }
        else if(G_sentry.Shoot_Turn_Mode == SentryRobot::RunningShoot && G_sentry.Shoot_KEYMOUSE.Key_Is_Shoot == 1 && G_aim_assist.m_data_receive_frame.m_data_f[2] > 0.5 && G_sentry.enemy_track_flag && G_sentry.Is_Heat_Safe() == true)
        {
            G_sentry.m_shoot_bullet_fps = SHOOT_BULLET_FREQUENCY_TEST_4;
        }
        else
        {
            G_sentry.m_shoot_bullet_fps = 0;
        }
      }
      //TODO:键鼠下的普通射击模式                         
       else  if(G_sentry.Shoot_KEYMOUSE.Key_Is_Assit == 0 && G_sentry.Is_Heat_Safe() == true)
      {
        if(G_sentry.Shoot_Turn_Mode == SentryRobot::SingleShoot && G_sentry.Shoot_KEYMOUSE.Key_Is_Shoot == 1 )
        {
            G_sentry.m_shoot_bullet_fps = SHOOT_BULLET_FREQUENCY_TEST_1;
        }
        else if(G_sentry.Shoot_Turn_Mode == SentryRobot::RunningShoot && G_sentry.Shoot_KEYMOUSE.Key_Is_Shoot == 1 )
        {
            G_sentry.m_shoot_bullet_fps = SHOOT_BULLET_FREQUENCY_TEST_4;
        }
        else
        {
            G_sentry.m_shoot_bullet_fps = 0;
        }

     }
    }


    //自动
    else if(G_sentry.shoot_mode == SentryRobot::SHOOT_AUTO && !G_sentry.shoot_insurance)
    {
        G_sentry.enemy_attackable_flag =    !((G_aim_assist.m_data_receive_frame.m_data_f[3] == 1 && G_aim_assist.m_enemy_1_res_flag)||
                                                (G_aim_assist.m_data_receive_frame.m_data_f[3] == 2 && G_aim_assist.m_enemy_2_res_flag)||
                                                (G_aim_assist.m_data_receive_frame.m_data_f[3] == 3 && G_aim_assist.m_enemy_3_res_flag)||
                                                (G_aim_assist.m_data_receive_frame.m_data_f[3] == 4 && G_aim_assist.m_enemy_4_res_flag)||
                                                (G_aim_assist.m_data_receive_frame.m_data_f[3] == 5 && G_aim_assist.m_enemy_5_res_flag)||
                                                (G_aim_assist.m_data_receive_frame.m_data_f[3] == 7 && G_aim_assist.m_enemy_7_res_flag));

        if(!G_sentry.shoot_cool_flag && G_aim_assist.m_data_receive_frame.m_data_f[2] > 0.5 && G_sentry.enemy_attackable_flag && G_sentry.enemy_track_flag)
            G_sentry.m_shoot_bullet_fps = SHOOT_BULLET_FREQUENCY_TEST_4;
    }
    //半自动
    else if(G_sentry.shoot_mode == SentryRobot::SHOOT_HALFAUTO && !G_sentry.shoot_insurance)
    {
        if (G_djirc.channel.Ch3 - RC_CH_VALUE_OFFSET < -RC_CH_VALUE_DEAD && G_aim_assist.m_data_receive_frame.m_data_f[2] > 0.5 && G_sentry.enemy_track_flag)
            G_sentry.m_shoot_bullet_fps = SHOOT_BULLET_FREQUENCY_TEST_1;
        else if(G_djirc.channel.Ch3 - RC_CH_VALUE_OFFSET > RC_CH_VALUE_DEAD && G_aim_assist.m_data_receive_frame.m_data_f[2] > 0.5 && !G_sentry.shoot_cool_flag && G_sentry.enemy_track_flag)
            G_sentry.m_shoot_bullet_fps = SHOOT_BULLET_FREQUENCY_TEST_4;
    }

    //安全
    else
    {
        G_sentry.m_shoot_bullet_cnt_target = 0;
        G_sentry.shoot_driver_start_angle = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_current;
        G_sentry.shoot_driver_angle_target = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_current;

        G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_target = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_current;
        G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_speed_target = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_speed_current;
        G_sentry.shoot_driver_angle_target = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_current;
        G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_speed_pid->m_output = 0;
        
        G_sentry.supply_bullut_reset_flag = 1;
    }

    // 拨弹电机帧率出问题时
    if(!G_gimbal.m_data_receive_frame.m_cdata[1])
        G_sentry.supply_bullut_reset_flag = 1;
    // 卡弹保护
    // static uint16_t bullet_clog_cnt;
    // static uint8_t bullet_clog_flag;
    // static uint16_t supply_bullet_stop_cnt;
    // if(fabs(G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_target = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_current
    //    - G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_speed_current) > SHOOT_DRIVER_SUPPLY_ANGLE_STEP)
    //     bullet_clog_cnt ++;
    // else 
    //     bullet_clog_cnt = 0;

    // if(bullet_clog_cnt > 2000)
    // {
    //     bullet_clog_cnt = 2000;
    //     bullet_clog_flag = 1;
    // }

    // if(bullet_clog_flag == 1)
    // {
    //     G_sentry.supply_bullut_reset_flag = 1;
    //     supply_bullet_stop_cnt ++;
    //     if(supply_bullet_stop_cnt > 5000)
    //     {
    //         bullet_clog_flag = 0;
    //         supply_bullet_stop_cnt = 0;
    //     }
    // }        
    
    
    //supply_bullet target
    if(G_sentry.supply_bullut_reset_flag)
    {
        G_sentry.m_shoot_bullet_cnt_target = 0; //TODO:【发射】？直接置0？后面发射时可以在上一次得到的angle_current的基础上进行正常角度变化发射，而不是从之前的shoot_cnt_target开始
        G_sentry.shoot_driver_start_angle = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_current;
        G_sentry.shoot_driver_angle_target = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_current;

        G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_target = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_angle_current;//修改
        G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_speed_target = G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_speed_current;
        G_sentry.supply_bullet_motor[SentryRobot::SHOOT_DRIVE_MOTOR]->m_speed_pid->m_output = 0;
    }
    else
    {
        if(G_sentry.m_shoot_bullet_fps != 0)
            G_sentry.IncreaseShootBulletTarget(G_system_monitor.SysTickTime);
            
        G_sentry.shoot_driver_angle_target = G_sentry.shoot_driver_start_angle - SHOOT_DRIVER_SUPPLY_ANGLE_STEP * (G_sentry.m_shoot_bullet_cnt_target);
        G_sentry.SetShootDriverTarget(G_sentry.shoot_driver_angle_target);
    }
}


//TODO:【系统】进行控制计算，并输出控制命令
/**
 *@brief execute the robot control
 *
 *@param
 */
void RobotControlExecute(void)
{
    // Execute the robot chassis control algorithm
    G_sentry.ExecuteChassisAlgorithm();

    // Execute the robot gimbal control algorithm
    G_sentry.ExecuteGimbalAlgorithm();

    // Execute the robot shoot control algorithm
    G_sentry.ExecuteShootAlgorithm();

    // Send the control commands to the actuators
    G_sentry.SendControlCommand();
}

//裁判系统UI绘制
//void UI_Client(void)
//{
//	static u8 Client_cnt = 0;
//	Client_cnt++;
//	if( Client_cnt >= 50 )	//步兵每1000ms接收数据的上限为3720bits
//	{
//		G_sentry.New_Client();  //客户端新建图层
//		Client_cnt = 0;
//	}
//}





